diff options
| -rw-r--r-- | source/blender/blenlib/intern/math_base.c | 8 | ||||
| -rw-r--r-- | source/blender/blenlib/intern/math_base_inline.c | 41 | ||||
| -rw-r--r-- | source/blender/blenlib/intern/math_color.c | 436 | ||||
| -rw-r--r-- | source/blender/blenlib/intern/math_color_inline.c | 17 | ||||
| -rw-r--r-- | source/blender/blenlib/intern/math_geom.c | 1737 | ||||
| -rw-r--r-- | source/blender/blenlib/intern/math_geom_inline.c | 66 | ||||
| -rw-r--r-- | source/blender/blenlib/intern/math_matrix.c | 1097 | ||||
| -rw-r--r-- | source/blender/blenlib/intern/math_rotation.c | 1540 | ||||
| -rw-r--r-- | source/blender/blenlib/intern/math_vector.c | 305 | ||||
| -rw-r--r-- | source/blender/blenlib/intern/math_vector_inline.c | 340 |
10 files changed, 2848 insertions, 2739 deletions
diff --git a/source/blender/blenlib/intern/math_base.c b/source/blender/blenlib/intern/math_base.c index f2df36202fe..9efcb3dbcae 100644 --- a/source/blender/blenlib/intern/math_base.c +++ b/source/blender/blenlib/intern/math_base.c @@ -60,7 +60,6 @@ double round(double x) double round(double x); #endif - /* from python 3.1 floatobject.c * ndigits must be between 0 and 21 */ double double_round(double x, int ndigits) @@ -69,7 +68,7 @@ double double_round(double x, int ndigits) if (ndigits >= 0) { pow1 = pow(10.0, (double)ndigits); pow2 = 1.0; - y = (x*pow1)*pow2; + y = (x * pow1) * pow2; /* if y overflows, then rounded value is exactly x */ if (!finite(y)) return x; @@ -81,9 +80,9 @@ double double_round(double x, int ndigits) } z = round(y); - if (fabs(y-z) == 0.5) + if (fabs(y - z) == 0.5) /* halfway between two integers; use round-half-even */ - z = 2.0*round(y/2.0); + z = 2.0 * round(y / 2.0); if (ndigits >= 0) z = (z / pow2) / pow1; @@ -93,4 +92,3 @@ double double_round(double x, int ndigits) /* if computation resulted in overflow, raise OverflowError */ return z; } - diff --git a/source/blender/blenlib/intern/math_base_inline.c b/source/blender/blenlib/intern/math_base_inline.c index d706f28e722..9e32ff5ad4e 100644 --- a/source/blender/blenlib/intern/math_base_inline.c +++ b/source/blender/blenlib/intern/math_base_inline.c @@ -43,61 +43,61 @@ MINLINE float sqrt3f(float f) { - if (f==0.0f) return 0.0f; - if (f<0) return (float)(-exp(log(-f)/3)); - else return (float)(exp(log(f)/3)); + if (f == 0.0f) return 0.0f; + if (f < 0) return (float)(-exp(log(-f) / 3)); + else return (float)(exp(log(f) / 3)); } MINLINE double sqrt3d(double d) { - if (d==0.0) return 0; - if (d<0) return -exp(log(-d)/3); - else return exp(log(d)/3); + if (d == 0.0) return 0; + if (d < 0) return -exp(log(-d) / 3); + else return exp(log(d) / 3); } MINLINE float saacos(float fac) { - if (fac<= -1.0f) return (float)M_PI; - else if (fac>=1.0f) return 0.0; + if (fac <= -1.0f) return (float)M_PI; + else if (fac >= 1.0f) return 0.0; else return (float)acos(fac); } MINLINE float saasin(float fac) { - if (fac<= -1.0f) return (float)-M_PI/2.0f; - else if (fac>=1.0f) return (float)M_PI/2.0f; + if (fac <= -1.0f) return (float)-M_PI / 2.0f; + else if (fac >= 1.0f) return (float)M_PI / 2.0f; else return (float)asin(fac); } MINLINE float sasqrt(float fac) { - if (fac<=0.0f) return 0.0f; + if (fac <= 0.0f) return 0.0f; return (float)sqrt(fac); } MINLINE float saacosf(float fac) { - if (fac<= -1.0f) return (float)M_PI; - else if (fac>=1.0f) return 0.0f; + if (fac <= -1.0f) return (float)M_PI; + else if (fac >= 1.0f) return 0.0f; else return (float)acosf(fac); } MINLINE float saasinf(float fac) { - if (fac<= -1.0f) return (float)-M_PI/2.0f; - else if (fac>=1.0f) return (float)M_PI/2.0f; + if (fac <= -1.0f) return (float)-M_PI / 2.0f; + else if (fac >= 1.0f) return (float)M_PI / 2.0f; else return (float)asinf(fac); } MINLINE float sasqrtf(float fac) { - if (fac<=0.0f) return 0.0f; + if (fac <= 0.0f) return 0.0f; return (float)sqrtf(fac); } MINLINE float interpf(float target, float origin, float fac) { - return (fac*target) + (1.0f-fac)*origin; + return (fac * target) + (1.0f - fac) * origin; } /* useful to calculate an even width shell, by taking the angle between 2 planes. @@ -139,20 +139,19 @@ MINLINE int power_of_2_min_i(int n) return n; } - MINLINE float minf(float a, float b) { - return (a < b)? a: b; + return (a < b) ? a : b; } MINLINE float maxf(float a, float b) { - return (a > b)? a: b; + return (a > b) ? a : b; } MINLINE float signf(float f) { - return (f < 0.f)? -1.f: 1.f; + return (f < 0.f) ? -1.f : 1.f; } diff --git a/source/blender/blenlib/intern/math_color.c b/source/blender/blenlib/intern/math_color.c index bef5b9e538b..7103ef6106d 100644 --- a/source/blender/blenlib/intern/math_color.c +++ b/source/blender/blenlib/intern/math_color.c @@ -41,51 +41,51 @@ void hsv_to_rgb(float h, float s, float v, float *r, float *g, float *b) int i; float f, p, q, t; - if (s==0.0f) { + if (s == 0.0f) { *r = v; *g = v; *b = v; } else { - h= (h - floorf(h))*6.0f; + h = (h - floorf(h)) * 6.0f; i = (int)floorf(h); f = h - i; - p = v*(1.0f-s); - q = v*(1.0f-(s*f)); - t = v*(1.0f-(s*(1.0f-f))); - + p = v * (1.0f - s); + q = v * (1.0f - (s * f)); + t = v * (1.0f - (s * (1.0f - f))); + switch (i) { - case 0 : - *r = v; - *g = t; - *b = p; - break; - case 1 : - *r = q; - *g = v; - *b = p; - break; - case 2 : - *r = p; - *g = v; - *b = t; - break; - case 3 : - *r = p; - *g = q; - *b = v; - break; - case 4 : - *r = t; - *g = p; - *b = v; - break; - case 5 : - *r = v; - *g = p; - *b = q; - break; + case 0: + *r = v; + *g = t; + *b = p; + break; + case 1: + *r = q; + *g = v; + *b = p; + break; + case 2: + *r = p; + *g = v; + *b = t; + break; + case 3: + *r = p; + *g = q; + *b = v; + break; + case 4: + *r = t; + *g = p; + *b = v; + break; + case 5: + *r = v; + *g = p; + *b = q; + break; } } } @@ -93,93 +93,95 @@ void hsv_to_rgb(float h, float s, float v, float *r, float *g, float *b) void rgb_to_yuv(float r, float g, float b, float *ly, float *lu, float *lv) { float y, u, v; - y= 0.299f*r + 0.587f*g + 0.114f*b; - u=-0.147f*r - 0.289f*g + 0.436f*b; - v= 0.615f*r - 0.515f*g - 0.100f*b; - - *ly=y; - *lu=u; - *lv=v; + y = 0.299f * r + 0.587f * g + 0.114f * b; + u = -0.147f * r - 0.289f * g + 0.436f * b; + v = 0.615f * r - 0.515f * g - 0.100f * b; + + *ly = y; + *lu = u; + *lv = v; } void yuv_to_rgb(float y, float u, float v, float *lr, float *lg, float *lb) { float r, g, b; - r=y+1.140f*v; - g=y-0.394f*u - 0.581f*v; - b=y+2.032f*u; - - *lr=r; - *lg=g; - *lb=b; + r = y + 1.140f * v; + g = y - 0.394f * u - 0.581f * v; + b = y + 2.032f * u; + + *lr = r; + *lg = g; + *lb = b; } -/* The RGB inputs are supposed gamma corrected and in the range 0 - 1.0f */ -/* Output YCC have a range of 16-235 and 16-240 except with JFIF_0_255 where the range is 0-255 */ +/* The RGB inputs are supposed gamma corrected and in the range 0 - 1.0f + * + * Output YCC have a range of 16-235 and 16-240 except with JFIF_0_255 where the range is 0-255 */ void rgb_to_ycc(float r, float g, float b, float *ly, float *lcb, float *lcr, int colorspace) { - float sr,sg, sb; + float sr, sg, sb; float y = 128.f, cr = 128.f, cb = 128.f; - - sr=255.0f*r; - sg=255.0f*g; - sb=255.0f*b; - + + sr = 255.0f * r; + sg = 255.0f * g; + sb = 255.0f * b; + switch (colorspace) { - case BLI_YCC_ITU_BT601 : - y=(0.257f*sr)+(0.504f*sg)+(0.098f*sb)+16.0f; - cb=(-0.148f*sr)-(0.291f*sg)+(0.439f*sb)+128.0f; - cr=(0.439f*sr)-(0.368f*sg)-(0.071f*sb)+128.0f; - break; - case BLI_YCC_ITU_BT709 : - y=(0.183f*sr)+(0.614f*sg)+(0.062f*sb)+16.0f; - cb=(-0.101f*sr)-(0.338f*sg)+(0.439f*sb)+128.0f; - cr=(0.439f*sr)-(0.399f*sg)-(0.040f*sb)+128.0f; - break; - case BLI_YCC_JFIF_0_255 : - y=(0.299f*sr)+(0.587f*sg)+(0.114f*sb); - cb=(-0.16874f*sr)-(0.33126f*sg)+(0.5f*sb)+128.0f; - cr=(0.5f*sr)-(0.41869f*sg)-(0.08131f*sb)+128.0f; - break; - default: - assert(!"invalid colorspace"); + case BLI_YCC_ITU_BT601: + y = (0.257f * sr) + (0.504f * sg) + (0.098f * sb) + 16.0f; + cb = (-0.148f * sr)-(0.291f * sg) + (0.439f * sb) + 128.0f; + cr = (0.439f * sr)-(0.368f * sg)-(0.071f * sb) + 128.0f; + break; + case BLI_YCC_ITU_BT709: + y = (0.183f * sr) + (0.614f * sg) + (0.062f * sb) + 16.0f; + cb = (-0.101f * sr)-(0.338f * sg) + (0.439f * sb) + 128.0f; + cr = (0.439f * sr)-(0.399f * sg)-(0.040f * sb) + 128.0f; + break; + case BLI_YCC_JFIF_0_255: + y = (0.299f * sr) + (0.587f * sg) + (0.114f * sb); + cb = (-0.16874f * sr)-(0.33126f * sg) + (0.5f * sb) + 128.0f; + cr = (0.5f * sr)-(0.41869f * sg)-(0.08131f * sb) + 128.0f; + break; + default: + assert(!"invalid colorspace"); } - - *ly=y; - *lcb=cb; - *lcr=cr; + + *ly = y; + *lcb = cb; + *lcr = cr; } /* YCC input have a range of 16-235 and 16-240 except with JFIF_0_255 where the range is 0-255 */ /* RGB outputs are in the range 0 - 1.0f */ + /* FIXME comment above must be wrong because BLI_YCC_ITU_BT601 y 16.0 cr 16.0 -> r -0.7009 */ void ycc_to_rgb(float y, float cb, float cr, float *lr, float *lg, float *lb, int colorspace) { float r = 128.f, g = 128.f, b = 128.f; - + switch (colorspace) { - case BLI_YCC_ITU_BT601 : - r=1.164f*(y-16.0f)+1.596f*(cr-128.0f); - g=1.164f*(y-16.0f)-0.813f*(cr-128.0f)-0.392f*(cb-128.0f); - b=1.164f*(y-16.0f)+2.017f*(cb-128.0f); - break; - case BLI_YCC_ITU_BT709 : - r=1.164f*(y-16.0f)+1.793f*(cr-128.0f); - g=1.164f*(y-16.0f)-0.534f*(cr-128.0f)-0.213f*(cb-128.0f); - b=1.164f*(y-16.0f)+2.115f*(cb-128.0f); - break; - case BLI_YCC_JFIF_0_255 : - r=y+1.402f*cr - 179.456f; - g=y-0.34414f*cb - 0.71414f*cr + 135.45984f; - b=y+1.772f*cb - 226.816f; - break; - default: - assert(!"invalid colorspace"); + case BLI_YCC_ITU_BT601: + r = 1.164f * (y - 16.0f) + 1.596f * (cr - 128.0f); + g = 1.164f * (y - 16.0f) - 0.813f * (cr - 128.0f) - 0.392f * (cb - 128.0f); + b = 1.164f * (y - 16.0f) + 2.017f * (cb - 128.0f); + break; + case BLI_YCC_ITU_BT709: + r = 1.164f * (y - 16.0f) + 1.793f * (cr - 128.0f); + g = 1.164f * (y - 16.0f) - 0.534f * (cr - 128.0f) - 0.213f * (cb - 128.0f); + b = 1.164f * (y - 16.0f) + 2.115f * (cb - 128.0f); + break; + case BLI_YCC_JFIF_0_255: + r = y + 1.402f * cr - 179.456f; + g = y - 0.34414f * cb - 0.71414f * cr + 135.45984f; + b = y + 1.772f * cb - 226.816f; + break; + default: + assert(!"invalid colorspace"); } - *lr=r/255.0f; - *lg=g/255.0f; - *lb=b/255.0f; + *lr = r / 255.0f; + *lg = g / 255.0f; + *lb = b / 255.0f; } void hex_to_rgb(char *hexcol, float *r, float *g, float *b) @@ -188,7 +190,7 @@ void hex_to_rgb(char *hexcol, float *r, float *g, float *b) if (hexcol[0] == '#') hexcol++; - if (sscanf(hexcol, "%02x%02x%02x", &ri, &gi, &bi)==3) { + if (sscanf(hexcol, "%02x%02x%02x", &ri, &gi, &bi) == 3) { *r = ri / 255.0f; *g = gi / 255.0f; *b = bi / 255.0f; @@ -198,7 +200,7 @@ void hex_to_rgb(char *hexcol, float *r, float *g, float *b) } else { /* avoid using un-initialized vars */ - *r= *g= *b= 0.0f; + *r = *g = *b = 0.0f; } } @@ -210,59 +212,59 @@ void rgb_to_hsv(float r, float g, float b, float *lh, float *ls, float *lv) cmax = r; cmin = r; - cmax = (g>cmax ? g:cmax); - cmin = (g<cmin ? g:cmin); - cmax = (b>cmax ? b:cmax); - cmin = (b<cmin ? b:cmin); + cmax = (g > cmax ? g : cmax); + cmin = (g < cmin ? g : cmin); + cmax = (b > cmax ? b : cmax); + cmin = (b < cmin ? b : cmin); - v = cmax; /* value */ + v = cmax; /* value */ if (cmax != 0.0f) - s = (cmax - cmin)/cmax; + s = (cmax - cmin) / cmax; else { s = 0.0f; } if (s == 0.0f) h = -1.0f; else { - cdelta = cmax-cmin; - rc = (cmax-r)/cdelta; - gc = (cmax-g)/cdelta; - bc = (cmax-b)/cdelta; - if (r==cmax) - h = bc-gc; + cdelta = cmax - cmin; + rc = (cmax - r) / cdelta; + gc = (cmax - g) / cdelta; + bc = (cmax - b) / cdelta; + if (r == cmax) + h = bc - gc; else - if (g==cmax) - h = 2.0f+rc-bc; + if (g == cmax) + h = 2.0f + rc - bc; else - h = 4.0f+gc-rc; - h = h*60.0f; + h = 4.0f + gc - rc; + h = h * 60.0f; if (h < 0.0f) h += 360.0f; } - + *ls = s; *lh = h / 360.0f; - if (*lh < 0.0f) *lh= 0.0f; + if (*lh < 0.0f) *lh = 0.0f; *lv = v; } void rgb_to_hsv_compat(float r, float g, float b, float *lh, float *ls, float *lv) { - float orig_h= *lh; - float orig_s= *ls; + float orig_h = *lh; + float orig_s = *ls; rgb_to_hsv(r, g, b, lh, ls, lv); if (*lv <= 0.0f) { - *lh= orig_h; - *ls= orig_s; + *lh = orig_h; + *ls = orig_s; } else if (*ls <= 0.0f) { - *lh= orig_h; + *lh = orig_h; } - if (*lh==0.0f && orig_h >= 1.0f) { - *lh= 1.0f; + if (*lh == 0.0f && orig_h >= 1.0f) { + *lh = 1.0f; } } @@ -270,22 +272,22 @@ void rgb_to_hsv_compat(float r, float g, float b, float *lh, float *ls, float *l void xyz_to_rgb(float xc, float yc, float zc, float *r, float *g, float *b, int colorspace) { - switch (colorspace) { - case BLI_XYZ_SMPTE: - *r = (3.50570f * xc) + (-1.73964f * yc) + (-0.544011f * zc); - *g = (-1.06906f * xc) + (1.97781f * yc) + (0.0351720f * zc); - *b = (0.0563117f * xc) + (-0.196994f * yc) + (1.05005f * zc); - break; - case BLI_XYZ_REC709_SRGB: - *r = (3.240476f * xc) + (-1.537150f * yc) + (-0.498535f * zc); - *g = (-0.969256f * xc) + (1.875992f * yc) + (0.041556f * zc); - *b = (0.055648f * xc) + (-0.204043f * yc) + (1.057311f * zc); - break; - case BLI_XYZ_CIE: - *r = (2.28783848734076f * xc) + (-0.833367677835217f * yc) + (-0.454470795871421f * zc); - *g = (-0.511651380743862f * xc) + (1.42275837632178f * yc) + (0.0888930017552939f * zc); - *b = (0.00572040983140966f * xc) + (-0.0159068485104036f * yc) + (1.0101864083734f * zc); - break; + switch (colorspace) { + case BLI_XYZ_SMPTE: + *r = (3.50570f * xc) + (-1.73964f * yc) + (-0.544011f * zc); + *g = (-1.06906f * xc) + (1.97781f * yc) + (0.0351720f * zc); + *b = (0.0563117f * xc) + (-0.196994f * yc) + (1.05005f * zc); + break; + case BLI_XYZ_REC709_SRGB: + *r = (3.240476f * xc) + (-1.537150f * yc) + (-0.498535f * zc); + *g = (-0.969256f * xc) + (1.875992f * yc) + (0.041556f * zc); + *b = (0.055648f * xc) + (-0.204043f * yc) + (1.057311f * zc); + break; + case BLI_XYZ_CIE: + *r = (2.28783848734076f * xc) + (-0.833367677835217f * yc) + (-0.454470795871421f * zc); + *g = (-0.511651380743862f * xc) + (1.42275837632178f * yc) + (0.0888930017552939f * zc); + *b = (0.00572040983140966f * xc) + (-0.0159068485104036f * yc) + (1.0101864083734f * zc); + break; } } @@ -298,58 +300,60 @@ unsigned int hsv_to_cpack(float h, float s, float v) short r, g, b; float rf, gf, bf; unsigned int col; - + hsv_to_rgb(h, s, v, &rf, &gf, &bf); - - r= (short)(rf*255.0f); - g= (short)(gf*255.0f); - b= (short)(bf*255.0f); - - col= ( r + (g*256) + (b*256*256) ); + + r = (short) (rf * 255.0f); + g = (short) (gf * 255.0f); + b = (short) (bf * 255.0f); + + col = (r + (g * 256) + (b * 256 * 256)); return col; } - unsigned int rgb_to_cpack(float r, float g, float b) { int ir, ig, ib; - - ir= (int)floor(255.0f*r); - if (ir<0) ir= 0; else if (ir>255) ir= 255; - ig= (int)floor(255.0f*g); - if (ig<0) ig= 0; else if (ig>255) ig= 255; - ib= (int)floor(255.0f*b); - if (ib<0) ib= 0; else if (ib>255) ib= 255; - - return (ir+ (ig*256) + (ib*256*256)); + + ir = (int)floor(255.0f * r); + if (ir < 0) ir = 0; + else if (ir > 255) ir = 255; + ig = (int)floor(255.0f * g); + if (ig < 0) ig = 0; + else if (ig > 255) ig = 255; + ib = (int)floor(255.0f * b); + if (ib < 0) ib = 0; + else if (ib > 255) ib = 255; + + return (ir + (ig * 256) + (ib * 256 * 256)); } void cpack_to_rgb(unsigned int col, float *r, float *g, float *b) { - - *r= (float)((col)&0xFF); + + *r = (float)((col)&0xFF); *r /= 255.0f; - *g= (float)(((col)>>8)&0xFF); + *g = (float)(((col) >> 8)&0xFF); *g /= 255.0f; - *b= (float)(((col)>>16)&0xFF); + *b = (float)(((col) >> 16)&0xFF); *b /= 255.0f; } void rgb_uchar_to_float(float col_r[3], const unsigned char col_ub[3]) { - col_r[0]= ((float)col_ub[0]) / 255.0f; - col_r[1]= ((float)col_ub[1]) / 255.0f; - col_r[2]= ((float)col_ub[2]) / 255.0f; + col_r[0] = ((float)col_ub[0]) / 255.0f; + col_r[1] = ((float)col_ub[1]) / 255.0f; + col_r[2] = ((float)col_ub[2]) / 255.0f; } void rgba_uchar_to_float(float col_r[4], const unsigned char col_ub[4]) { - col_r[0]= ((float)col_ub[0]) / 255.0f; - col_r[1]= ((float)col_ub[1]) / 255.0f; - col_r[2]= ((float)col_ub[2]) / 255.0f; - col_r[3]= ((float)col_ub[3]) / 255.0f; + col_r[0] = ((float)col_ub[0]) / 255.0f; + col_r[1] = ((float)col_ub[1]) / 255.0f; + col_r[2] = ((float)col_ub[2]) / 255.0f; + col_r[3] = ((float)col_ub[3]) / 255.0f; } void rgb_float_to_uchar(unsigned char col_r[3], const float col_f[3]) @@ -373,15 +377,15 @@ void gamma_correct(float *c, float gamma) float rec709_to_linearrgb(float c) { if (c < 0.081f) - return (c < 0.0f)? 0.0f: c * (1.0f/4.5f); + return (c < 0.0f) ? 0.0f : c * (1.0f / 4.5f); else - return powf((c + 0.099f)*(1.0f/1.099f), (1.0f/0.45f)); + return powf((c + 0.099f) * (1.0f / 1.099f), (1.0f / 0.45f)); } float linearrgb_to_rec709(float c) { if (c < 0.018f) - return (c < 0.0f)? 0.0f: c * 4.5f; + return (c < 0.0f) ? 0.0f : c * 4.5f; else return 1.099f * powf(c, 0.45f) - 0.099f; } @@ -389,31 +393,31 @@ float linearrgb_to_rec709(float c) float srgb_to_linearrgb(float c) { if (c < 0.04045f) - return (c < 0.0f)? 0.0f: c * (1.0f/12.92f); + return (c < 0.0f) ? 0.0f : c * (1.0f / 12.92f); else - return powf((c + 0.055f)*(1.0f/1.055f), 2.4f); + return powf((c + 0.055f) * (1.0f / 1.055f), 2.4f); } float linearrgb_to_srgb(float c) { if (c < 0.0031308f) - return (c < 0.0f)? 0.0f: c * 12.92f; + return (c < 0.0f) ? 0.0f : c * 12.92f; else - return 1.055f * powf(c, 1.0f/2.4f) - 0.055f; + return 1.055f * powf(c, 1.0f / 2.4f) - 0.055f; } void minmax_rgb(short c[]) { - if (c[0]>255) c[0]=255; - else if (c[0]<0) c[0]=0; - if (c[1]>255) c[1]=255; - else if (c[1]<0) c[1]=0; - if (c[2]>255) c[2]=255; - else if (c[2]<0) c[2]=0; + if (c[0] > 255) c[0] = 255; + else if (c[0] < 0) c[0] = 0; + if (c[1] > 255) c[1] = 255; + else if (c[1] < 0) c[1] = 0; + if (c[2] > 255) c[2] = 255; + else if (c[2] < 0) c[2] = 0; } /*If the requested RGB shade contains a negative weight for - * one of the primaries, it lies outside the color gamut + * one of the primaries, it lies outside the color gamut * accessible from the given triple of primaries. Desaturate * it by adding white, equal quantities of R, G, and B, enough * to make RGB all positive. The function returns 1 if the @@ -432,31 +436,33 @@ int constrain_rgb(float *r, float *g, float *b) /* Add just enough white to make r, g, b all positive. */ if (w > 0) { - *r += w; *g += w; *b += w; - return 1; /* Color modified to fit RGB gamut */ + *r += w; + *g += w; + *b += w; + return 1; /* Color modified to fit RGB gamut */ } - return 0; /* Color within RGB gamut */ + return 0; /* Color within RGB gamut */ } float rgb_to_grayscale(const float rgb[3]) { - return 0.3f*rgb[0] + 0.58f*rgb[1] + 0.12f*rgb[2]; + return 0.3f * rgb[0] + 0.58f * rgb[1] + 0.12f * rgb[2]; } unsigned char rgb_to_grayscale_byte(const unsigned char rgb[3]) { - return (76*(unsigned short)rgb[0] + 148*(unsigned short)rgb[1] + 31*(unsigned short)rgb[2]) / 255; + return (76 * (unsigned short) rgb[0] + 148 * (unsigned short) rgb[1] + 31 * (unsigned short) rgb[2]) / 255; } float rgb_to_luma(const float rgb[3]) { - return 0.299f*rgb[0] + 0.587f*rgb[1] + 0.114f*rgb[2]; + return 0.299f * rgb[0] + 0.587f * rgb[1] + 0.114f * rgb[2]; } unsigned char rgb_to_luma_byte(const unsigned char rgb[3]) { - return (76*(unsigned short)rgb[0] + 150*(unsigned short)rgb[1] + 29*(unsigned short)rgb[2]) / 255; + return (76 * (unsigned short) rgb[0] + 150 * (unsigned short) rgb[1] + 29 * (unsigned short) rgb[2]) / 255; } /* ********************************* lift/gamma/gain / ASC-CDL conversion ********************************* */ @@ -464,13 +470,13 @@ unsigned char rgb_to_luma_byte(const unsigned char rgb[3]) void lift_gamma_gain_to_asc_cdl(float *lift, float *gamma, float *gain, float *offset, float *slope, float *power) { int c; - for (c=0; c<3; c++) { - offset[c]= lift[c]*gain[c]; - slope[c]= gain[c]*(1.0f-lift[c]); + for (c = 0; c < 3; c++) { + offset[c] = lift[c] * gain[c]; + slope[c] = gain[c] * (1.0f - lift[c]); if (gamma[c] == 0) - power[c]= FLT_MAX; + power[c] = FLT_MAX; else - power[c]= 1.0f/gamma[c]; + power[c] = 1.0f / gamma[c]; } } @@ -480,21 +486,21 @@ void lift_gamma_gain_to_asc_cdl(float *lift, float *gamma, float *gain, float *o void rgb_float_set_hue_float_offset(float rgb[3], float hue_offset) { float hsv[3]; - - rgb_to_hsv(rgb[0], rgb[1], rgb[2], hsv, hsv+1, hsv+2); - - hsv[0]+= hue_offset; - if (hsv[0] > 1.0f) hsv[0] -= 1.0f; - else if (hsv[0] < 0.0f) hsv[0] += 1.0f; - - hsv_to_rgb(hsv[0], hsv[1], hsv[2], rgb, rgb+1, rgb+2); + + rgb_to_hsv(rgb[0], rgb[1], rgb[2], hsv, hsv + 1, hsv + 2); + + hsv[0] += hue_offset; + if (hsv[0] > 1.0f) hsv[0] -= 1.0f; + else if (hsv[0] < 0.0f) hsv[0] += 1.0f; + + hsv_to_rgb(hsv[0], hsv[1], hsv[2], rgb, rgb + 1, rgb + 2); } /* Applies an hue offset to a byte rgb color */ void rgb_byte_set_hue_float_offset(unsigned char rgb[3], float hue_offset) { float rgb_float[3]; - + rgb_uchar_to_float(rgb_float, rgb); rgb_float_set_hue_float_offset(rgb_float, hue_offset); rgb_float_to_uchar(rgb, rgb_float); @@ -527,16 +533,17 @@ static unsigned short hipart(const float f) static float index_to_float(const unsigned short i) { + union { float f; unsigned short us[2]; } tmp; /* positive and negative zeros, and all gradual underflow, turn into zero: */ - if (i<0x80 || (i >= 0x8000 && i < 0x8080)) return 0; + if (i < 0x80 || (i >= 0x8000 && i < 0x8080)) return 0; /* All NaN's and infinity turn into the largest possible legal float: */ - if (i>=0x7f80 && i<0x8000) return FLT_MAX; - if (i>=0xff80) return -FLT_MAX; + if (i >= 0x7f80 && i < 0x8000) return FLT_MAX; + if (i >= 0xff80) return -FLT_MAX; #ifdef __BIG_ENDIAN__ tmp.us[0] = i; @@ -551,7 +558,7 @@ static float index_to_float(const unsigned short i) void BLI_init_srgb_conversion(void) { - static int initialized= 0; + static int initialized = 0; int i, b; if (initialized) return; @@ -559,19 +566,18 @@ void BLI_init_srgb_conversion(void) /* Fill in the lookup table to convert floats to bytes: */ for (i = 0; i < 0x10000; i++) { - float f = linearrgb_to_srgb(index_to_float(i))*255.0f; + float f = linearrgb_to_srgb(index_to_float(i)) * 255.0f; if (f <= 0) BLI_color_to_srgb_table[i] = 0; - else if (f < 255) BLI_color_to_srgb_table[i] = (unsigned short)(f*0x100+0.5f); + else if (f < 255) BLI_color_to_srgb_table[i] = (unsigned short) (f * 0x100 + 0.5f); else BLI_color_to_srgb_table[i] = 0xff00; } /* Fill in the lookup table to convert bytes to float: */ for (b = 0; b <= 255; b++) { - float f = srgb_to_linearrgb(((float)b)*(1.0f/255.0f)); + float f = srgb_to_linearrgb(((float)b) * (1.0f / 255.0f)); BLI_color_from_srgb_table[b] = f; i = hipart(f); /* replace entries so byte->float->byte does not change the data: */ - BLI_color_to_srgb_table[i] = b*0x100; + BLI_color_to_srgb_table[i] = b * 0x100; } } - diff --git a/source/blender/blenlib/intern/math_color_inline.c b/source/blender/blenlib/intern/math_color_inline.c index 4f7a197e79b..1aebd8bda11 100644 --- a/source/blender/blenlib/intern/math_color_inline.c +++ b/source/blender/blenlib/intern/math_color_inline.c @@ -93,7 +93,7 @@ MINLINE void srgb_to_linearrgb_predivide_v4(float linear[4], const float srgb[4] } else { alpha = srgb[3]; - inv_alpha = 1.0f/alpha; + inv_alpha = 1.0f / alpha; } linear[0] = srgb_to_linearrgb(srgb[0] * inv_alpha) * alpha; @@ -112,7 +112,7 @@ MINLINE void linearrgb_to_srgb_predivide_v4(float srgb[4], const float linear[4] } else { alpha = linear[3]; - inv_alpha = 1.0f/alpha; + inv_alpha = 1.0f / alpha; } srgb[0] = linearrgb_to_srgb(linear[0] * inv_alpha) * alpha; @@ -128,6 +128,7 @@ extern unsigned short BLI_color_to_srgb_table[0x10000]; MINLINE unsigned short to_srgb_table_lookup(const float f) { + union { float f; unsigned short us[2]; @@ -159,11 +160,11 @@ MINLINE void linearrgb_to_srgb_ushort4_predivide(unsigned short srgb[4], const f } alpha = linear[3]; - inv_alpha = 1.0f/alpha; + inv_alpha = 1.0f / alpha; - for (i=0; i<3; ++i) { + for (i = 0; i < 3; ++i) { t = linear[i] * inv_alpha; - srgb[i] = (t < 1.0f)? (unsigned short)(to_srgb_table_lookup(t) * alpha) : FTOUSHORT(linearrgb_to_srgb(t) * alpha); + srgb[i] = (t < 1.0f) ? (unsigned short) (to_srgb_table_lookup(t) * alpha) : FTOUSHORT(linearrgb_to_srgb(t) * alpha); } srgb[3] = FTOUSHORT(linear[3]); @@ -174,7 +175,7 @@ MINLINE void srgb_to_linearrgb_uchar4(float linear[4], const unsigned char srgb[ linear[0] = BLI_color_from_srgb_table[srgb[0]]; linear[1] = BLI_color_from_srgb_table[srgb[1]]; linear[2] = BLI_color_from_srgb_table[srgb[2]]; - linear[3] = srgb[3] * (1.0f/255.0f); + linear[3] = srgb[3] * (1.0f / 255.0f); } MINLINE void srgb_to_linearrgb_uchar4_predivide(float linear[4], const unsigned char srgb[4]) @@ -187,8 +188,8 @@ MINLINE void srgb_to_linearrgb_uchar4_predivide(float linear[4], const unsigned return; } - for (i=0; i<4; i++) - fsrgb[i] = srgb[i] * (1.0f/255.0f); + for (i = 0; i < 4; i++) + fsrgb[i] = srgb[i] * (1.0f / 255.0f); srgb_to_linearrgb_predivide_v4(linear, fsrgb); } diff --git a/source/blender/blenlib/intern/math_geom.c b/source/blender/blenlib/intern/math_geom.c index aa62df5ae3e..f84a79c544d 100644 --- a/source/blender/blenlib/intern/math_geom.c +++ b/source/blender/blenlib/intern/math_geom.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 ***** @@ -39,31 +39,31 @@ void cent_tri_v3(float cent[3], const float v1[3], const float v2[3], const float v3[3]) { - cent[0]= 0.33333f*(v1[0]+v2[0]+v3[0]); - cent[1]= 0.33333f*(v1[1]+v2[1]+v3[1]); - cent[2]= 0.33333f*(v1[2]+v2[2]+v3[2]); + cent[0] = 0.33333f * (v1[0] + v2[0] + v3[0]); + cent[1] = 0.33333f * (v1[1] + v2[1] + v3[1]); + cent[2] = 0.33333f * (v1[2] + v2[2] + v3[2]); } void cent_quad_v3(float cent[3], const float v1[3], const float v2[3], const float v3[3], const float v4[3]) { - cent[0]= 0.25f*(v1[0]+v2[0]+v3[0]+v4[0]); - cent[1]= 0.25f*(v1[1]+v2[1]+v3[1]+v4[1]); - cent[2]= 0.25f*(v1[2]+v2[2]+v3[2]+v4[2]); + cent[0] = 0.25f * (v1[0] + v2[0] + v3[0] + v4[0]); + cent[1] = 0.25f * (v1[1] + v2[1] + v3[1] + v4[1]); + cent[2] = 0.25f * (v1[2] + v2[2] + v3[2] + v4[2]); } float normal_tri_v3(float n[3], const float v1[3], const float v2[3], const float v3[3]) { - float n1[3],n2[3]; + float n1[3], n2[3]; - n1[0]= v1[0]-v2[0]; - n2[0]= v2[0]-v3[0]; - n1[1]= v1[1]-v2[1]; - n2[1]= v2[1]-v3[1]; - n1[2]= v1[2]-v2[2]; - n2[2]= v2[2]-v3[2]; - n[0]= n1[1]*n2[2]-n1[2]*n2[1]; - n[1]= n1[2]*n2[0]-n1[0]*n2[2]; - n[2]= n1[0]*n2[1]-n1[1]*n2[0]; + n1[0] = v1[0] - v2[0]; + n2[0] = v2[0] - v3[0]; + n1[1] = v1[1] - v2[1]; + n2[1] = v2[1] - v3[1]; + n1[2] = v1[2] - v2[2]; + n2[2] = v2[2] - v3[2]; + n[0] = n1[1] * n2[2] - n1[2] * n2[1]; + n[1] = n1[2] * n2[0] - n1[0] * n2[2]; + n[2] = n1[0] * n2[1] - n1[1] * n2[0]; return normalize_v3(n); } @@ -71,88 +71,90 @@ float normal_tri_v3(float n[3], const float v1[3], const float v2[3], const floa float normal_quad_v3(float n[3], const float v1[3], const float v2[3], const float v3[3], const float v4[3]) { /* real cross! */ - float n1[3],n2[3]; + float n1[3], n2[3]; - n1[0]= v1[0]-v3[0]; - n1[1]= v1[1]-v3[1]; - n1[2]= v1[2]-v3[2]; + n1[0] = v1[0] - v3[0]; + n1[1] = v1[1] - v3[1]; + n1[2] = v1[2] - v3[2]; - n2[0]= v2[0]-v4[0]; - n2[1]= v2[1]-v4[1]; - n2[2]= v2[2]-v4[2]; + n2[0] = v2[0] - v4[0]; + n2[1] = v2[1] - v4[1]; + n2[2] = v2[2] - v4[2]; - n[0]= n1[1]*n2[2]-n1[2]*n2[1]; - n[1]= n1[2]*n2[0]-n1[0]*n2[2]; - n[2]= n1[0]*n2[1]-n1[1]*n2[0]; + n[0] = n1[1] * n2[2] - n1[2] * n2[1]; + n[1] = n1[2] * n2[0] - n1[0] * n2[2]; + n[2] = n1[0] * n2[1] - n1[1] * n2[0]; return normalize_v3(n); } float area_tri_v2(const float v1[2], const float v2[2], const float v3[2]) { - return 0.5f * fabsf((v1[0]-v2[0])*(v2[1]-v3[1]) + (v1[1]-v2[1])*(v3[0]-v2[0])); + return 0.5f * fabsf((v1[0] - v2[0]) * (v2[1] - v3[1]) + (v1[1] - v2[1]) * (v3[0] - v2[0])); } float area_tri_signed_v2(const float v1[2], const float v2[2], const float v3[2]) { - return 0.5f * ((v1[0]-v2[0])*(v2[1]-v3[1]) + (v1[1]-v2[1])*(v3[0]-v2[0])); + return 0.5f * ((v1[0] - v2[0]) * (v2[1] - v3[1]) + (v1[1] - v2[1]) * (v3[0] - v2[0])); } -float area_quad_v3(const float v1[3], const float v2[3], const float v3[3], const float v4[3]) /* only convex Quadrilaterals */ +/* only convex Quadrilaterals */ +float area_quad_v3(const float v1[3], const float v2[3], const float v3[3], const float v4[3]) { float len, vec1[3], vec2[3], n[3]; sub_v3_v3v3(vec1, v2, v1); sub_v3_v3v3(vec2, v4, v1); cross_v3_v3v3(n, vec1, vec2); - len= normalize_v3(n); + len = normalize_v3(n); sub_v3_v3v3(vec1, v4, v3); sub_v3_v3v3(vec2, v2, v3); cross_v3_v3v3(n, vec1, vec2); - len+= normalize_v3(n); + len += normalize_v3(n); - return (len/2.0f); + return (len / 2.0f); } -float area_tri_v3(const float v1[3], const float v2[3], const float v3[3]) /* Triangles */ +/* Triangles */ +float area_tri_v3(const float v1[3], const float v2[3], const float v3[3]) { float len, vec1[3], vec2[3], n[3]; sub_v3_v3v3(vec1, v3, v2); sub_v3_v3v3(vec2, v1, v2); cross_v3_v3v3(n, vec1, vec2); - len= normalize_v3(n); + len = normalize_v3(n); - return (len/2.0f); + return (len / 2.0f); } float area_poly_v3(int nr, float verts[][3], const float normal[3]) { float x, y, z, area, max; float *cur, *prev; - int a, px=0, py=1; + int a, px = 0, py = 1; /* first: find dominant axis: 0==X, 1==Y, 2==Z * don't use 'axis_dominant_v3()' because we need max axis too */ - x= fabsf(normal[0]); - y= fabsf(normal[1]); - z= fabsf(normal[2]); + x = fabsf(normal[0]); + y = fabsf(normal[1]); + z = fabsf(normal[2]); max = MAX3(x, y, z); - if (max==y) py=2; - else if (max==x) { - px=1; - py= 2; + if (max == y) py = 2; + else if (max == x) { + px = 1; + py = 2; } /* The Trapezium Area Rule */ - 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= verts[a]; - cur= verts[a+1]; + 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 = verts[a]; + cur = verts[a + 1]; } return fabsf(0.5f * area / max); @@ -160,18 +162,18 @@ float area_poly_v3(int nr, float verts[][3], const float normal[3]) /********************************* Distance **********************************/ -/* distance v1 to line v2-v3 */ -/* using Hesse formula, NO LINE PIECE! */ +/* distance v1 to line v2-v3 + * using Hesse formula, NO LINE PIECE! */ float dist_to_line_v2(const float v1[2], const float v2[2], const float v3[2]) { - float a[2],deler; + float a[2], deler; - a[0]= v2[1]-v3[1]; - a[1]= v3[0]-v2[0]; - deler= (float)sqrt(a[0]*a[0]+a[1]*a[1]); - if (deler== 0.0f) return 0; + a[0] = v2[1] - v3[1]; + a[1] = v3[0] - v2[0]; + deler = (float)sqrt(a[0] * a[0] + a[1] * a[1]); + if (deler == 0.0f) return 0; - return fabsf((v1[0]-v2[0])*a[0]+(v1[1]-v2[1])*a[1])/deler; + return fabsf((v1[0] - v2[0]) * a[0]+(v1[1] - v2[1]) * a[1]) / deler; } @@ -179,33 +181,33 @@ float dist_to_line_v2(const float v1[2], const float v2[2], const float v3[2]) float dist_to_line_segment_v2(const float v1[2], const float v2[2], const float v3[2]) { float labda, rc[2], pt[2], len; - - rc[0]= v3[0]-v2[0]; - rc[1]= v3[1]-v2[1]; - len= rc[0]*rc[0]+ rc[1]*rc[1]; - if (len==0.0f) { - rc[0]= v1[0]-v2[0]; - rc[1]= v1[1]-v2[1]; - return (float)(sqrt(rc[0]*rc[0]+ rc[1]*rc[1])); - } - - labda= (rc[0]*(v1[0]-v2[0]) + rc[1]*(v1[1]-v2[1]))/len; + + rc[0] = v3[0] - v2[0]; + rc[1] = v3[1] - v2[1]; + len = rc[0] * rc[0] + rc[1] * rc[1]; + if (len == 0.0f) { + rc[0] = v1[0] - v2[0]; + rc[1] = v1[1] - v2[1]; + return (float)(sqrt(rc[0] * rc[0] + rc[1] * rc[1])); + } + + labda = (rc[0] * (v1[0] - v2[0]) + rc[1] * (v1[1] - v2[1])) / len; if (labda <= 0.0f) { - pt[0]= v2[0]; - pt[1]= v2[1]; + pt[0] = v2[0]; + pt[1] = v2[1]; } else if (labda >= 1.0f) { - pt[0]= v3[0]; - pt[1]= v3[1]; + pt[0] = v3[0]; + pt[1] = v3[1]; } else { - pt[0]= labda*rc[0]+v2[0]; - pt[1]= labda*rc[1]+v2[1]; + pt[0] = labda * rc[0] + v2[0]; + pt[1] = labda * rc[1] + v2[1]; } - rc[0]= pt[0]-v1[0]; - rc[1]= pt[1]-v1[1]; - return sqrtf(rc[0]*rc[0]+ rc[1]*rc[1]); + rc[0] = pt[0] - v1[0]; + rc[1] = pt[1] - v1[1]; + return sqrtf(rc[0] * rc[0] + rc[1] * rc[1]); } /* point closest to v1 on line v2-v3 in 2D */ @@ -213,7 +215,7 @@ void closest_to_line_segment_v2(float close_r[2], const float p[2], const float { float lambda, cp[2]; - lambda= closest_to_line_v2(cp,p, l1, l2); + lambda = closest_to_line_v2(cp, p, l1, l2); if (lambda <= 0.0f) copy_v2_v2(close_r, l1); @@ -228,7 +230,7 @@ void closest_to_line_segment_v3(float close_r[3], const float v1[3], const float { float lambda, cp[3]; - lambda= closest_to_line_v3(cp,v1, v2, v3); + lambda = closest_to_line_v3(cp, v1, v2, v3); if (lambda <= 0.0f) copy_v3_v3(close_r, v2); @@ -245,14 +247,13 @@ void closest_to_line_segment_v3(float close_r[3], const float v1[3], const float * pt: the point that you want the nearest of */ -// const float norm[3], const float coord[3], const float point[3], float dst_r[3] void closest_to_plane_v3(float close_r[3], const float plane_co[3], const float plane_no_unit[3], const float pt[3]) { float temp[3]; float dotprod; sub_v3_v3v3(temp, pt, plane_co); - dotprod= dot_v3v3(temp, plane_no_unit); + dotprod = dot_v3v3(temp, plane_no_unit); close_r[0] = pt[0] - (plane_no_unit[0] * dotprod); close_r[1] = pt[1] - (plane_no_unit[1] * dotprod); @@ -296,16 +297,16 @@ float dist_to_line_segment_v3(const float v1[3], const float v2[3], const float int isect_line_line_v2_int(const int v1[2], const int v2[2], const int v3[2], const int v4[2]) { float div, labda, mu; - - div= (float)((v2[0]-v1[0])*(v4[1]-v3[1])-(v2[1]-v1[1])*(v4[0]-v3[0])); - if (div==0.0f) return ISECT_LINE_LINE_COLINEAR; - - labda= ((float)(v1[1]-v3[1])*(v4[0]-v3[0])-(v1[0]-v3[0])*(v4[1]-v3[1]))/div; - - mu= ((float)(v1[1]-v3[1])*(v2[0]-v1[0])-(v1[0]-v3[0])*(v2[1]-v1[1]))/div; - - if (labda>=0.0f && labda<=1.0f && mu>=0.0f && mu<=1.0f) { - if (labda==0.0f || labda==1.0f || mu==0.0f || mu==1.0f) return ISECT_LINE_LINE_EXACT; + + div = (float)((v2[0] - v1[0]) * (v4[1] - v3[1])-(v2[1] - v1[1]) * (v4[0] - v3[0])); + if (div == 0.0f) return ISECT_LINE_LINE_COLINEAR; + + labda = ((float)(v1[1] - v3[1]) * (v4[0] - v3[0])-(v1[0] - v3[0]) * (v4[1] - v3[1])) / div; + + mu = ((float)(v1[1] - v3[1]) * (v2[0] - v1[0])-(v1[0] - v3[0]) * (v2[1] - v1[1])) / div; + + if (labda >= 0.0f && labda <= 1.0f && mu >= 0.0f && mu <= 1.0f) { + if (labda == 0.0f || labda == 1.0f || mu == 0.0f || mu == 1.0f) return ISECT_LINE_LINE_EXACT; return ISECT_LINE_LINE_CROSS; } return ISECT_LINE_LINE_NONE; @@ -315,16 +316,16 @@ int isect_line_line_v2_int(const int v1[2], const int v2[2], const int v3[2], co int isect_line_line_v2(const float v1[2], const float v2[2], const float v3[2], const float v4[2]) { float div, labda, mu; - - div= (v2[0]-v1[0])*(v4[1]-v3[1])-(v2[1]-v1[1])*(v4[0]-v3[0]); - if (div==0.0f) return ISECT_LINE_LINE_COLINEAR; - - labda= ((float)(v1[1]-v3[1])*(v4[0]-v3[0])-(v1[0]-v3[0])*(v4[1]-v3[1]))/div; - - mu= ((float)(v1[1]-v3[1])*(v2[0]-v1[0])-(v1[0]-v3[0])*(v2[1]-v1[1]))/div; - - if (labda>=0.0f && labda<=1.0f && mu>=0.0f && mu<=1.0f) { - if (labda==0.0f || labda==1.0f || mu==0.0f || mu==1.0f) return ISECT_LINE_LINE_EXACT; + + div = (v2[0] - v1[0]) * (v4[1] - v3[1])-(v2[1] - v1[1]) * (v4[0] - v3[0]); + if (div == 0.0f) return ISECT_LINE_LINE_COLINEAR; + + labda = ((float)(v1[1] - v3[1]) * (v4[0] - v3[0])-(v1[0] - v3[0]) * (v4[1] - v3[1])) / div; + + mu = ((float)(v1[1] - v3[1]) * (v2[0] - v1[0])-(v1[0] - v3[0]) * (v2[1] - v1[1])) / div; + + if (labda >= 0.0f && labda <= 1.0f && mu >= 0.0f && mu <= 1.0f) { + if (labda == 0.0f || labda == 1.0f || mu == 0.0f || mu == 1.0f) return ISECT_LINE_LINE_EXACT; return ISECT_LINE_LINE_CROSS; } return ISECT_LINE_LINE_NONE; @@ -338,25 +339,25 @@ int isect_seg_seg_v2_point(const float v1[2], const float v2[2], const float v3[ { float a1, a2, b1, b2, c1, c2, d; float u, v; - const float eps= 0.000001f; + const float eps = 0.000001f; - a1= v2[0]-v1[0]; - b1= v4[0]-v3[0]; - c1= v1[0]-v4[0]; + a1 = v2[0] - v1[0]; + b1 = v4[0] - v3[0]; + c1 = v1[0] - v4[0]; - a2= v2[1]-v1[1]; - b2= v4[1]-v3[1]; - c2= v1[1]-v4[1]; + a2 = v2[1] - v1[1]; + b2 = v4[1] - v3[1]; + c2 = v1[1] - v4[1]; - d= a1*b2-a2*b1; + d = a1 * b2 - a2 * b1; - if (d==0) { - if (a1*c2-a2*c1==0.0f && b1*c2-b2*c1==0.0f) { /* equal lines */ + if (d == 0) { + if (a1 * c2 - a2 * c1 == 0.0f && b1 * c2 - b2 * c1 == 0.0f) { /* equal lines */ float a[2], b[2], c[2]; float u2; - if (len_v2v2(v1, v2)==0.0f) { - if (len_v2v2(v3, v4)>eps) { + if (len_v2v2(v1, v2) == 0.0f) { + if (len_v2v2(v3, v4) > eps) { /* use non-point segment as basis */ SWAP(const float *, v1, v3); SWAP(const float *, v2, v4); @@ -375,14 +376,14 @@ int isect_seg_seg_v2_point(const float v1[2], const float v2[2], const float v3[ sub_v2_v2v2(a, v3, v1); sub_v2_v2v2(b, v2, v1); sub_v2_v2v2(c, v2, v1); - u= dot_v2v2(a, b) / dot_v2v2(c, c); + u = dot_v2v2(a, b) / dot_v2v2(c, c); sub_v2_v2v2(a, v4, v1); - u2= dot_v2v2(a, b) / dot_v2v2(c, c); + u2 = dot_v2v2(a, b) / dot_v2v2(c, c); - if (u>u2) SWAP(float, u, u2); + if (u > u2) SWAP(float, u, u2); - if (u>1.0f+eps || u2<-eps) return -1; /* non-ovlerlapping segments */ + if (u > 1.0f + eps || u2<-eps) return -1; /* non-ovlerlapping segments */ else if (maxf(0.0f, u) == minf(1.0f, u2)) { /* one common point: can return result */ interp_v2_v2v2(vi, v1, v2, maxf(0, u)); return 1; @@ -393,10 +394,10 @@ int isect_seg_seg_v2_point(const float v1[2], const float v2[2], const float v3[ return -1; } - u= (c2*b1-b2*c1)/d; - v= (c1*a2-a1*c2)/d; + u = (c2 * b1 - b2 * c1) / d; + v = (c1 * a2 - a1 * c2) / d; - if (u>=-eps && u<=1.0f+eps && v>=-eps && v<=1.0f+eps) { /* intersection */ + if (u >= -eps && u <= 1.0f + eps && v >= -eps && v <= 1.0f + eps) { /* intersection */ interp_v2_v2v2(vi, v1, v2, u); return 1; } @@ -427,20 +428,20 @@ int isect_line_sphere_v3(const float l1[3], const float l2[3], * Paul Bourke pbourke@swin.edu.au */ - const float ldir[3]= { + const float ldir[3] = { l2[0] - l1[0], l2[1] - l1[1], l2[2] - l1[2] }; - const float a= dot_v3v3(ldir, ldir); + const float a = dot_v3v3(ldir, ldir); - const float b= 2.0f * + const float b = 2.0f * (ldir[0] * (l1[0] - sp[0]) + ldir[1] * (l1[1] - sp[1]) + ldir[2] * (l1[2] - sp[2])); - const float c= + const float c = dot_v3v3(sp, sp) + dot_v3v3(l1, l1) - (2.0f * dot_v3v3(sp, l1)) - @@ -461,7 +462,7 @@ int isect_line_sphere_v3(const float l1[3], const float l2[3], return 1; } else if (i > 0.0f) { - const float i_sqrt= sqrt(i); /* avoid calc twice */ + const float i_sqrt = sqrt(i); /* avoid calc twice */ /* first intersection */ mu = (-b + i_sqrt) / (2.0f * a); @@ -483,18 +484,16 @@ int isect_line_sphere_v2(const float l1[2], const float l2[2], const float sp[2], const float r, float r_p1[2], float r_p2[2]) { - const float ldir[2]= { - l2[0] - l1[0], - l2[1] - l1[1] - }; + const float ldir[2] = {l2[0] - l1[0], + l2[1] - l1[1]}; - const float a= dot_v2v2(ldir, ldir); + const float a = dot_v2v2(ldir, ldir); - const float b= 2.0f * + const float b = 2.0f * (ldir[0] * (l1[0] - sp[0]) + - ldir[1] * (l1[1] - sp[1])); + ldir[1] * (l1[1] - sp[1])); - const float c= + const float c = dot_v2v2(sp, sp) + dot_v2v2(l1, l1) - (2.0f * dot_v2v2(sp, l1)) - @@ -515,7 +514,7 @@ int isect_line_sphere_v2(const float l1[2], const float l2[2], return 1; } else if (i > 0.0f) { - const float i_sqrt= sqrt(i); /* avoid calc twice */ + const float i_sqrt = sqrt(i); /* avoid calc twice */ /* first intersection */ mu = (-b + i_sqrt) / (2.0f * a); @@ -537,7 +536,7 @@ int isect_line_sphere_v2(const float l1[2], const float l2[2], * 1: intersection */ static short IsectLLPt2Df(const float x0, const float y0, const float x1, const float y1, - const float x2, const float y2, const float x3, const float y3, float *xi,float *yi) + const float x2, const float y2, const float x3, const float y3, float *xi, float *yi) { /* @@ -548,92 +547,93 @@ static short IsectLLPt2Df(const float x0, const float y0, const float x1, const * applies the math, but we don't need speed since this is a * pre-processing step */ - float c1,c2, // constants of linear equations - det_inv, // the inverse of the determinant of the coefficient - m1,m2; // the slopes of each line + float c1, c2; /* constants of linear equations */ + float det_inv; /* the inverse of the determinant of the coefficient */ + float m1, m2; /* the slopes of each line */ /* * compute slopes, note the cludge for infinity, however, this will * be close enough */ - if (fabs(x1-x0) > 0.000001) - m1 = (y1-y0) / (x1-x0); + if (fabs(x1 - x0) > 0.000001) + m1 = (y1 - y0) / (x1 - x0); else - return -1; /*m1 = (float) 1e+10;*/ // close enough to infinity + return -1; /*m1 = (float)1e+10;*/ // close enough to infinity - if (fabs(x3-x2) > 0.000001) - m2 = (y3-y2) / (x3-x2); + if (fabs(x3 - x2) > 0.000001) + m2 = (y3 - y2) / (x3 - x2); else - return -1; /*m2 = (float) 1e+10;*/ // close enough to infinity + return -1; /*m2 = (float)1e+10;*/ // close enough to infinity - if (fabs(m1-m2) < 0.000001) + if (fabs(m1 - m2) < 0.000001) return -1; /* parallel lines */ - -// compute constants - c1 = (y0-m1*x0); - c2 = (y2-m2*x2); + // compute constants + + c1 = (y0 - m1 * x0); + c2 = (y2 - m2 * x2); -// compute the inverse of the determinate + // compute the inverse of the determinate det_inv = 1.0f / (-m1 + m2); -// use Kramers rule to compute xi and yi + // use Kramers rule to compute xi and yi - *xi= ((-c2 + c1) *det_inv); - *yi= ((m2*c1 - m1*c2) *det_inv); - - return 1; -} // end Intersect_Lines + *xi = ((-c2 + c1) * det_inv); + *yi = ((m2 * c1 - m1 * c2) * det_inv); + + return 1; +} /* point in tri */ int isect_point_tri_v2(const float pt[2], const float v1[2], const float v2[2], const float v3[2]) { - if (line_point_side_v2(v1,v2,pt)>=0.0f) { - if (line_point_side_v2(v2,v3,pt)>=0.0f) { - if (line_point_side_v2(v3,v1,pt)>=0.0f) { + if (line_point_side_v2(v1, v2, pt) >= 0.0f) { + if (line_point_side_v2(v2, v3, pt) >= 0.0f) { + if (line_point_side_v2(v3, v1, pt) >= 0.0f) { return 1; } } } else { - if (! (line_point_side_v2(v2,v3,pt)>=0.0f)) { - if (! (line_point_side_v2(v3,v1,pt)>=0.0f)) { + if (!(line_point_side_v2(v2, v3, pt) >= 0.0f)) { + if (!(line_point_side_v2(v3, v1, pt) >= 0.0f)) { return -1; } } } - + return 0; } + /* point in quad - only convex quads */ int isect_point_quad_v2(const float pt[2], const float v1[2], const float v2[2], const float v3[2], const float v4[2]) { - if (line_point_side_v2(v1,v2,pt)>=0.0f) { - if (line_point_side_v2(v2,v3,pt)>=0.0f) { - if (line_point_side_v2(v3,v4,pt)>=0.0f) { - if (line_point_side_v2(v4,v1,pt)>=0.0f) { + if (line_point_side_v2(v1, v2, pt) >= 0.0f) { + if (line_point_side_v2(v2, v3, pt) >= 0.0f) { + if (line_point_side_v2(v3, v4, pt) >= 0.0f) { + if (line_point_side_v2(v4, v1, pt) >= 0.0f) { return 1; } } } } else { - if (! (line_point_side_v2(v2,v3,pt)>=0.0f)) { - if (! (line_point_side_v2(v3,v4,pt)>=0.0f)) { - if (! (line_point_side_v2(v4,v1,pt)>=0.0f)) { + if (!(line_point_side_v2(v2, v3, pt) >= 0.0f)) { + if (!(line_point_side_v2(v3, v4, pt) >= 0.0f)) { + if (!(line_point_side_v2(v4, v1, pt) >= 0.0f)) { return -1; } } } } - + return 0; } /* moved from effect.c * test if the line starting at p1 ending at p2 intersects the triangle v0..v2 - * return non zero if it does + * return non zero if it does */ int isect_line_tri_v3(const float p1[3], const float p2[3], const float v0[3], const float v1[3], const float v2[3], @@ -642,39 +642,40 @@ int isect_line_tri_v3(const float p1[3], const float p2[3], float p[3], s[3], d[3], e1[3], e2[3], q[3]; float a, f, u, v; - + sub_v3_v3v3(e1, v1, v0); sub_v3_v3v3(e2, v2, v0); sub_v3_v3v3(d, p2, p1); - + cross_v3_v3v3(p, d, e2); a = dot_v3v3(e1, p); if ((a > -0.000001f) && (a < 0.000001f)) return 0; - f = 1.0f/a; - + f = 1.0f / a; + sub_v3_v3v3(s, p1, v0); - + u = f * dot_v3v3(s, p); - if ((u < 0.0f)||(u > 1.0f)) return 0; - + if ((u < 0.0f) || (u > 1.0f)) return 0; + cross_v3_v3v3(q, s, e1); - + v = f * dot_v3v3(d, q); - if ((v < 0.0f)||((u + v) > 1.0f)) return 0; + if ((v < 0.0f) || ((u + v) > 1.0f)) return 0; *r_lambda = f * dot_v3v3(e2, q); - if ((*r_lambda < 0.0f)||(*r_lambda > 1.0f)) return 0; + if ((*r_lambda < 0.0f) || (*r_lambda > 1.0f)) return 0; if (r_uv) { - r_uv[0]= u; - r_uv[1]= v; + r_uv[0] = u; + r_uv[1] = v; } - + return 1; } + /* moved from effect.c * test if the ray starting at p1 going in d direction intersects the triangle v0..v2 - * return non zero if it does + * return non zero if it does */ int isect_ray_tri_v3(const float p1[3], const float d[3], const float v0[3], const float v1[3], const float v2[3], @@ -682,35 +683,35 @@ int isect_ray_tri_v3(const float p1[3], const float d[3], { float p[3], s[3], e1[3], e2[3], q[3]; float a, f, u, v; - + sub_v3_v3v3(e1, v1, v0); sub_v3_v3v3(e2, v2, v0); - + cross_v3_v3v3(p, d, e2); a = dot_v3v3(e1, p); /* note: these values were 0.000001 in 2.4x but for projection snapping on * a human head (1BU==1m), subsurf level 2, this gave many errors - campbell */ if ((a > -0.00000001f) && (a < 0.00000001f)) return 0; - f = 1.0f/a; - + f = 1.0f / a; + sub_v3_v3v3(s, p1, v0); - + u = f * dot_v3v3(s, p); - if ((u < 0.0f)||(u > 1.0f)) return 0; + if ((u < 0.0f) || (u > 1.0f)) return 0; cross_v3_v3v3(q, s, e1); - + v = f * dot_v3v3(d, q); - if ((v < 0.0f)||((u + v) > 1.0f)) return 0; + if ((v < 0.0f) || ((u + v) > 1.0f)) return 0; *r_lambda = f * dot_v3v3(e2, q); if ((*r_lambda < 0.0f)) return 0; if (r_uv) { - r_uv[0]= u; - r_uv[1]= v; + r_uv[0] = u; + r_uv[1] = v; } - + return 1; } @@ -724,20 +725,20 @@ int isect_ray_plane_v3(const float p1[3], const float d[3], sub_v3_v3v3(e1, v1, v0); sub_v3_v3v3(e2, v2, v0); - + cross_v3_v3v3(p, d, e2); a = dot_v3v3(e1, p); /* note: these values were 0.000001 in 2.4x but for projection snapping on * a human head (1BU==1m), subsurf level 2, this gave many errors - campbell */ if ((a > -0.00000001f) && (a < 0.00000001f)) return 0; - f = 1.0f/a; - + f = 1.0f / a; + sub_v3_v3v3(s, p1, v0); - + /* u = f * dot_v3v3(s, p); */ /*UNUSED*/ cross_v3_v3v3(q, s, e1); - + /* v = f * dot_v3v3(d, q); */ /*UNUSED*/ *r_lambda = f * dot_v3v3(e2, q); @@ -759,24 +760,24 @@ int isect_ray_tri_epsilon_v3(const float p1[3], const float d[3], cross_v3_v3v3(p, d, e2); a = dot_v3v3(e1, p); if (a == 0.0f) return 0; - f = 1.0f/a; + f = 1.0f / a; sub_v3_v3v3(s, p1, v0); u = f * dot_v3v3(s, p); - if ((u < -epsilon)||(u > 1.0f+epsilon)) return 0; + if ((u < -epsilon) || (u > 1.0f + epsilon)) return 0; cross_v3_v3v3(q, s, e1); v = f * dot_v3v3(d, q); - if ((v < -epsilon)||((u + v) > 1.0f+epsilon)) return 0; + if ((v < -epsilon) || ((u + v) > 1.0f + epsilon)) return 0; *r_lambda = f * dot_v3v3(e2, q); if ((*r_lambda < 0.0f)) return 0; if (uv) { - uv[0]= u; - uv[1]= v; + uv[0] = u; + uv[1] = v; } return 1; @@ -789,50 +790,52 @@ int isect_ray_tri_threshold_v3(const float p1[3], const float d[3], float p[3], s[3], e1[3], e2[3], q[3]; float a, f, u, v; float du = 0, dv = 0; - + sub_v3_v3v3(e1, v1, v0); sub_v3_v3v3(e2, v2, v0); - + cross_v3_v3v3(p, d, e2); a = dot_v3v3(e1, p); if ((a > -0.000001f) && (a < 0.000001f)) return 0; - f = 1.0f/a; - + f = 1.0f / a; + sub_v3_v3v3(s, p1, v0); - + cross_v3_v3v3(q, s, e1); *r_lambda = f * dot_v3v3(e2, q); if ((*r_lambda < 0.0f)) return 0; - + u = f * dot_v3v3(s, p); v = f * dot_v3v3(d, q); - + if (u < 0) du = u; if (u > 1) du = u - 1; if (v < 0) dv = v; if (v > 1) dv = v - 1; if (u > 0 && v > 0 && u + v > 1) { float t = u + v - 1; - du = u - t/2; - dv = v - t/2; + du = u - t / 2; + dv = v - t / 2; } mul_v3_fl(e1, du); mul_v3_fl(e2, dv); - + if (dot_v3v3(e1, e1) + dot_v3v3(e2, e2) > threshold * threshold) { return 0; } if (r_uv) { - r_uv[0]= u; - r_uv[1]= v; + r_uv[0] = u; + r_uv[1] = v; } - + return 1; } -int isect_line_plane_v3(float out[3], const float l1[3], const float l2[3], const float plane_co[3], const float plane_no[3], const short no_flip) +int isect_line_plane_v3(float out[3], + const float l1[3], const float l2[3], + const float plane_co[3], const float plane_no[3], const short no_flip) { float l_vec[3]; /* l1 -> l2 normalized vector */ float p_no[3]; /* 'plane_no' normalized */ @@ -843,7 +846,7 @@ int isect_line_plane_v3(float out[3], const float l1[3], const float l2[3], cons normalize_v3(l_vec); normalize_v3_v3(p_no, plane_no); - dot= dot_v3v3(l_vec, p_no); + dot = dot_v3v3(l_vec, p_no); if (dot == 0.0f) { return 0; } @@ -854,7 +857,7 @@ int isect_line_plane_v3(float out[3], const float l1[3], const float l2[3], cons /* for predictable flipping since the plane is only used to * define a direction, ignore its flipping and aligned with 'l_vec' */ if (dot < 0.0f) { - dot= -dot; + dot = -dot; negate_v3(p_no); } @@ -864,7 +867,7 @@ int isect_line_plane_v3(float out[3], const float l1[3], const float l2[3], cons /* treat line like a ray, when 'no_flip' is set */ if (no_flip && dist < 0.0f) { - dist= -dist; + dist = -dist; } mul_v3_fl(l_vec, dist / dot); @@ -889,20 +892,21 @@ void isect_plane_plane_v3(float r_isect_co[3], float r_isect_no[3], /* Adapted from the paper by Kasper Fauerby */ + /* "Improved Collision detection and Response" */ static int getLowestRoot(const float a, const float b, const float c, const float maxR, float *root) { // Check if a solution exists - float determinant = b*b - 4.0f*a*c; + float determinant = b * b - 4.0f * a * c; // If determinant is negative it means no solutions. if (determinant >= 0.0f) { // calculate the two roots: (if determinant == 0 then // x1==x2 but lets disregard that slight optimization) float sqrtD = (float)sqrt(determinant); - float r1 = (-b - sqrtD) / (2.0f*a); - float r2 = (-b + sqrtD) / (2.0f*a); - + float r1 = (-b - sqrtD) / (2.0f * a); + float r2 = (-b + sqrtD) / (2.0f * a); + // Sort so x1 <= x2 if (r1 > r2) SWAP(float, r1, r2); @@ -924,30 +928,29 @@ static int getLowestRoot(const float a, const float b, const float c, const floa return 0; } -int isect_sweeping_sphere_tri_v3( - const float p1[3], const float p2[3], const float radius, - const float v0[3], const float v1[3], const float v2[3], - float *r_lambda, float ipoint[3]) +int isect_sweeping_sphere_tri_v3(const float p1[3], const float p2[3], const float radius, + const float v0[3], const float v1[3], const float v2[3], + float *r_lambda, float ipoint[3]) { float e1[3], e2[3], e3[3], point[3], vel[3], /*dist[3],*/ nor[3], temp[3], bv[3]; - float a, b, c, d, e, x, y, z, radius2=radius*radius; - float elen2,edotv,edotbv,nordotv; + float a, b, c, d, e, x, y, z, radius2 = radius * radius; + float elen2, edotv, edotbv, nordotv; float newLambda; - int found_by_sweep=0; + int found_by_sweep = 0; - sub_v3_v3v3(e1,v1,v0); - sub_v3_v3v3(e2,v2,v0); - sub_v3_v3v3(vel,p2,p1); + sub_v3_v3v3(e1, v1, v0); + sub_v3_v3v3(e2, v2, v0); + sub_v3_v3v3(vel, p2, p1); -/*---test plane of tri---*/ - cross_v3_v3v3(nor,e1,e2); + /*---test plane of tri---*/ + cross_v3_v3v3(nor, e1, e2); normalize_v3(nor); /* flip normal */ - if (dot_v3v3(nor,vel)>0.0f) negate_v3(nor); - - a=dot_v3v3(p1,nor)-dot_v3v3(v0,nor); - nordotv=dot_v3v3(nor,vel); + if (dot_v3v3(nor, vel) > 0.0f) negate_v3(nor); + + a = dot_v3v3(p1, nor) - dot_v3v3(v0, nor); + nordotv = dot_v3v3(nor, vel); if (fabsf(nordotv) < 0.000001f) { if (fabsf(a) >= radius) { @@ -955,13 +958,13 @@ int isect_sweeping_sphere_tri_v3( } } else { - float t0=(-a+radius)/nordotv; - float t1=(-a-radius)/nordotv; + float t0 = (-a + radius) / nordotv; + float t1 = (-a - radius) / nordotv; - if (t0>t1) + if (t0 > t1) SWAP(float, t0, t1); - if (t0>1.0f || t1<0.0f) return 0; + if (t0 > 1.0f || t1 < 0.0f) return 0; /* clamp to [0,1] */ CLAMP(t0, 0.0f, 1.0f); @@ -970,115 +973,115 @@ int isect_sweeping_sphere_tri_v3( /*---test inside of tri---*/ /* plane intersection point */ - point[0] = p1[0] + vel[0]*t0 - nor[0]*radius; - point[1] = p1[1] + vel[1]*t0 - nor[1]*radius; - point[2] = p1[2] + vel[2]*t0 - nor[2]*radius; + point[0] = p1[0] + vel[0] * t0 - nor[0] * radius; + point[1] = p1[1] + vel[1] * t0 - nor[1] * radius; + point[2] = p1[2] + vel[2] * t0 - nor[2] * radius; /* is the point in the tri? */ - a=dot_v3v3(e1,e1); - b=dot_v3v3(e1,e2); - c=dot_v3v3(e2,e2); + a = dot_v3v3(e1, e1); + b = dot_v3v3(e1, e2); + c = dot_v3v3(e2, e2); - sub_v3_v3v3(temp,point,v0); - d=dot_v3v3(temp,e1); - e=dot_v3v3(temp,e2); - - x=d*c-e*b; - y=e*a-d*b; - z=x+y-(a*c-b*b); + sub_v3_v3v3(temp, point, v0); + d = dot_v3v3(temp, e1); + e = dot_v3v3(temp, e2); + + x = d * c - e * b; + y = e * a - d * b; + z = x + y - (a * c - b * b); if (z <= 0.0f && (x >= 0.0f && y >= 0.0f)) { - //(((unsigned int)z)& ~(((unsigned int)x)|((unsigned int)y))) & 0x80000000) { - *r_lambda=t0; - copy_v3_v3(ipoint,point); + //(((unsigned int)z)& ~(((unsigned int)x)|((unsigned int)y))) & 0x80000000) { + *r_lambda = t0; + copy_v3_v3(ipoint, point); return 1; } } - *r_lambda=1.0f; + *r_lambda = 1.0f; -/*---test points---*/ - a=dot_v3v3(vel,vel); + /*---test points---*/ + a = dot_v3v3(vel, vel); /*v0*/ - sub_v3_v3v3(temp,p1,v0); - b=2.0f*dot_v3v3(vel,temp); - c=dot_v3v3(temp,temp)-radius2; + sub_v3_v3v3(temp, p1, v0); + b = 2.0f * dot_v3v3(vel, temp); + c = dot_v3v3(temp, temp) - radius2; if (getLowestRoot(a, b, c, *r_lambda, r_lambda)) { - copy_v3_v3(ipoint,v0); - found_by_sweep=1; + copy_v3_v3(ipoint, v0); + found_by_sweep = 1; } /*v1*/ - sub_v3_v3v3(temp,p1,v1); - b=2.0f*dot_v3v3(vel,temp); - c=dot_v3v3(temp,temp)-radius2; + sub_v3_v3v3(temp, p1, v1); + b = 2.0f * dot_v3v3(vel, temp); + c = dot_v3v3(temp, temp) - radius2; if (getLowestRoot(a, b, c, *r_lambda, r_lambda)) { - copy_v3_v3(ipoint,v1); - found_by_sweep=1; + copy_v3_v3(ipoint, v1); + found_by_sweep = 1; } - + /*v2*/ - sub_v3_v3v3(temp,p1,v2); - b=2.0f*dot_v3v3(vel,temp); - c=dot_v3v3(temp,temp)-radius2; + sub_v3_v3v3(temp, p1, v2); + b = 2.0f * dot_v3v3(vel, temp); + c = dot_v3v3(temp, temp) - radius2; if (getLowestRoot(a, b, c, *r_lambda, r_lambda)) { - copy_v3_v3(ipoint,v2); - found_by_sweep=1; + copy_v3_v3(ipoint, v2); + found_by_sweep = 1; } -/*---test edges---*/ - sub_v3_v3v3(e3,v2,v1); //wasnt yet calculated + /*---test edges---*/ + sub_v3_v3v3(e3, v2, v1); //wasnt yet calculated /*e1*/ - sub_v3_v3v3(bv,v0,p1); + sub_v3_v3v3(bv, v0, p1); - elen2 = dot_v3v3(e1,e1); - edotv = dot_v3v3(e1,vel); - edotbv = dot_v3v3(e1,bv); + elen2 = dot_v3v3(e1, e1); + edotv = dot_v3v3(e1, vel); + edotbv = dot_v3v3(e1, bv); - a=elen2*(-dot_v3v3(vel,vel))+edotv*edotv; - b=2.0f*(elen2*dot_v3v3(vel,bv)-edotv*edotbv); - c=elen2*(radius2-dot_v3v3(bv,bv))+edotbv*edotbv; + a = elen2 * (-dot_v3v3(vel, vel)) + edotv * edotv; + b = 2.0f * (elen2 * dot_v3v3(vel, bv) - edotv * edotbv); + c = elen2 * (radius2 - dot_v3v3(bv, bv)) + edotbv * edotbv; if (getLowestRoot(a, b, c, *r_lambda, &newLambda)) { - e=(edotv*newLambda-edotbv)/elen2; + e = (edotv * newLambda - edotbv) / elen2; if (e >= 0.0f && e <= 1.0f) { *r_lambda = newLambda; - copy_v3_v3(ipoint,e1); - mul_v3_fl(ipoint,e); + copy_v3_v3(ipoint, e1); + mul_v3_fl(ipoint, e); add_v3_v3(ipoint, v0); - found_by_sweep=1; + found_by_sweep = 1; } } /*e2*/ /*bv is same*/ - elen2 = dot_v3v3(e2,e2); - edotv = dot_v3v3(e2,vel); - edotbv = dot_v3v3(e2,bv); + elen2 = dot_v3v3(e2, e2); + edotv = dot_v3v3(e2, vel); + edotbv = dot_v3v3(e2, bv); - a=elen2*(-dot_v3v3(vel,vel))+edotv*edotv; - b=2.0f*(elen2*dot_v3v3(vel,bv)-edotv*edotbv); - c=elen2*(radius2-dot_v3v3(bv,bv))+edotbv*edotbv; + a = elen2 * (-dot_v3v3(vel, vel)) + edotv * edotv; + b = 2.0f * (elen2 * dot_v3v3(vel, bv) - edotv * edotbv); + c = elen2 * (radius2 - dot_v3v3(bv, bv)) + edotbv * edotbv; if (getLowestRoot(a, b, c, *r_lambda, &newLambda)) { - e=(edotv*newLambda-edotbv)/elen2; + e = (edotv * newLambda - edotbv) / elen2; if (e >= 0.0f && e <= 1.0f) { *r_lambda = newLambda; - copy_v3_v3(ipoint,e2); - mul_v3_fl(ipoint,e); + copy_v3_v3(ipoint, e2); + mul_v3_fl(ipoint, e); add_v3_v3(ipoint, v0); - found_by_sweep=1; + found_by_sweep = 1; } } @@ -1088,36 +1091,37 @@ int isect_sweeping_sphere_tri_v3( /* edotv = dot_v3v3(e1,vel); */ /* UNUSED */ /* edotbv = dot_v3v3(e1,bv); */ /* UNUSED */ - sub_v3_v3v3(bv,v1,p1); - elen2 = dot_v3v3(e3,e3); - edotv = dot_v3v3(e3,vel); - edotbv = dot_v3v3(e3,bv); + sub_v3_v3v3(bv, v1, p1); + elen2 = dot_v3v3(e3, e3); + edotv = dot_v3v3(e3, vel); + edotbv = dot_v3v3(e3, bv); - a=elen2*(-dot_v3v3(vel,vel))+edotv*edotv; - b=2.0f*(elen2*dot_v3v3(vel,bv)-edotv*edotbv); - c=elen2*(radius2-dot_v3v3(bv,bv))+edotbv*edotbv; + a = elen2 * (-dot_v3v3(vel, vel)) + edotv * edotv; + b = 2.0f * (elen2 * dot_v3v3(vel, bv) - edotv * edotbv); + c = elen2 * (radius2 - dot_v3v3(bv, bv)) + edotbv * edotbv; if (getLowestRoot(a, b, c, *r_lambda, &newLambda)) { - e=(edotv*newLambda-edotbv)/elen2; + e = (edotv * newLambda - edotbv) / elen2; if (e >= 0.0f && e <= 1.0f) { *r_lambda = newLambda; - copy_v3_v3(ipoint,e3); - mul_v3_fl(ipoint,e); + copy_v3_v3(ipoint, e3); + mul_v3_fl(ipoint, e); add_v3_v3(ipoint, v1); - found_by_sweep=1; + found_by_sweep = 1; } } return found_by_sweep; } + int isect_axial_line_tri_v3(const int axis, const float p1[3], const float p2[3], const float v0[3], const float v1[3], const float v2[3], float *r_lambda) { float p[3], e1[3], e2[3]; float u, v, f; - int a0=axis, a1=(axis+1)%3, a2=(axis+2)%3; + int a0 = axis, a1 = (axis + 1) % 3, a2 = (axis + 2) % 3; //return isect_line_tri_v3(p1,p2,v0,v1,v2,lambda); @@ -1128,29 +1132,29 @@ int isect_axial_line_tri_v3(const int axis, const float p1[3], const float p2[3] //if(MAX3(v0[a2],v1[a2],v2[a2]) < p1[a2]) return 0; ///* then a full intersection test */ - - sub_v3_v3v3(e1,v1,v0); - sub_v3_v3v3(e2,v2,v0); - sub_v3_v3v3(p,v0,p1); - f= (e2[a1]*e1[a2]-e2[a2]*e1[a1]); + sub_v3_v3v3(e1, v1, v0); + sub_v3_v3v3(e2, v2, v0); + sub_v3_v3v3(p, v0, p1); + + f = (e2[a1] * e1[a2] - e2[a2] * e1[a1]); if ((f > -0.000001f) && (f < 0.000001f)) return 0; - v= (p[a2]*e1[a1]-p[a1]*e1[a2])/f; - if ((v < 0.0f)||(v > 1.0f)) return 0; - - f= e1[a1]; + v = (p[a2] * e1[a1] - p[a1] * e1[a2]) / f; + if ((v < 0.0f) || (v > 1.0f)) return 0; + + f = e1[a1]; if ((f > -0.000001f) && (f < 0.000001f)) { - f= e1[a2]; + f = e1[a2]; if ((f > -0.000001f) && (f < 0.000001f)) return 0; - u= (-p[a2]-v*e2[a2])/f; + u = (-p[a2] - v * e2[a2]) / f; } else - u= (-p[a1]-v*e2[a1])/f; + u = (-p[a1] - v * e2[a1]) / f; if ((u < 0.0f) || ((u + v) > 1.0f)) return 0; - *r_lambda = (p[a0]+u*e1[a0]+v*e2[a0])/(p2[a0]-p1[a0]); + *r_lambda = (p[a0] + u * e1[a0] + v * e2[a0]) / (p2[a0] - p1[a0]); if ((*r_lambda < 0.0f) || (*r_lambda > 1.0f)) return 0; @@ -1160,13 +1164,13 @@ int isect_axial_line_tri_v3(const int axis, const float p1[3], const float p2[3] /* Returns the number of point of interests * 0 - lines are colinear * 1 - lines are coplanar, i1 is set to intersection - * 2 - i1 and i2 are the nearest points on line 1 (v1, v2) and line 2 (v3, v4) respectively + * 2 - i1 and i2 are the nearest points on line 1 (v1, v2) and line 2 (v3, v4) respectively * */ int isect_line_line_v3(const float v1[3], const float v2[3], const float v3[3], const float v4[3], float i1[3], float i2[3]) { float a[3], b[3], c[3], ab[3], cb[3], dir1[3], dir2[3]; float d; - + sub_v3_v3v3(c, v3, v1); sub_v3_v3v3(a, v2, v1); sub_v3_v3v3(b, v4, v3); @@ -1189,7 +1193,7 @@ int isect_line_line_v3(const float v1[3], const float v2[3], const float v3[3], mul_v3_fl(a, dot_v3v3(cb, ab) / dot_v3v3(ab, ab)); add_v3_v3v3(i1, v1, a); copy_v3_v3(i2, i1); - + return 1; /* one intersection only */ } /* if not */ @@ -1205,7 +1209,7 @@ int isect_line_line_v3(const float v1[3], const float v2[3], const float v3[3], /* for the first line, offset the second line until it is coplanar */ add_v3_v3v3(v3t, v3, t); add_v3_v3v3(v4t, v4, t); - + sub_v3_v3v3(c, v3t, v1); sub_v3_v3v3(a, v2, v1); sub_v3_v3v3(b, v4t, v3t); @@ -1218,19 +1222,19 @@ int isect_line_line_v3(const float v1[3], const float v2[3], const float v3[3], /* for the second line, just substract the offset from the first intersection point */ sub_v3_v3v3(i2, i1, t); - + return 2; /* two nearest points */ } -} +} /* Intersection point strictly between the two lines - * 0 when no intersection is found + * 0 when no intersection is found * */ int isect_line_line_strict_v3(const float v1[3], const float v2[3], const float v3[3], const float v4[3], float vi[3], float *r_lambda) { float a[3], b[3], c[3], ab[3], cb[3], ca[3], dir1[3], dir2[3]; float d; - + sub_v3_v3v3(c, v3, v1); sub_v3_v3v3(a, v2, v1); sub_v3_v3v3(b, v4, v3); @@ -1254,9 +1258,9 @@ int isect_line_line_strict_v3(const float v1[3], const float v2[3], const float f1 = dot_v3v3(cb, ab) / dot_v3v3(ab, ab); f2 = dot_v3v3(ca, ab) / dot_v3v3(ab, ab); - + if (f1 >= 0 && f1 <= 1 && - f2 >= 0 && f2 <= 1) + f2 >= 0 && f2 <= 1) { mul_v3_fl(a, f1); add_v3_v3v3(vi, v1, a); @@ -1272,12 +1276,12 @@ int isect_line_line_strict_v3(const float v1[3], const float v2[3], const float else { return 0; } -} +} int isect_aabb_aabb_v3(const float min1[3], const float max1[3], const float min2[3], const float max2[3]) { - return (min1[0]<max2[0] && min1[1]<max2[1] && min1[2]<max2[2] && - min2[0]<max1[0] && min2[1]<max1[1] && min2[2]<max1[2]); + return (min1[0] < max2[0] && min1[1] < max2[1] && min1[2] < max2[2] && + min2[0] < max1[0] && min2[1] < max1[1] && min2[2] < max1[2]); } /* find closest point to p on line through l1,l2 and return lambda, @@ -1285,22 +1289,22 @@ int isect_aabb_aabb_v3(const float min1[3], const float max1[3], const float min */ float closest_to_line_v3(float cp[3], const float p[3], const float l1[3], const float l2[3]) { - float h[3],u[3],lambda; + float h[3], u[3], lambda; sub_v3_v3v3(u, l2, l1); sub_v3_v3v3(h, p, l1); - lambda = dot_v3v3(u,h)/dot_v3v3(u,u); + lambda = dot_v3v3(u, h) / dot_v3v3(u, u); cp[0] = l1[0] + u[0] * lambda; cp[1] = l1[1] + u[1] * lambda; cp[2] = l1[2] + u[2] * lambda; return lambda; } -float closest_to_line_v2(float cp[2],const float p[2], const float l1[2], const float l2[2]) +float closest_to_line_v2(float cp[2], const float p[2], const float l1[2], const float l2[2]) { - float h[2],u[2],lambda; + float h[2], u[2], lambda; sub_v2_v2v2(u, l2, l1); sub_v2_v2v2(h, p, l1); - lambda = dot_v2v2(u,h)/dot_v2v2(u,u); + lambda = dot_v2v2(u, h) / dot_v2v2(u, u); cp[0] = l1[0] + u[0] * lambda; cp[1] = l1[1] + u[1] * lambda; return lambda; @@ -1309,10 +1313,10 @@ float closest_to_line_v2(float cp[2],const float p[2], const float l1[2], const /* little sister we only need to know lambda */ float line_point_factor_v3(const float p[3], const float l1[3], const float l2[3]) { - float h[3],u[3]; + float h[3], u[3]; sub_v3_v3v3(u, l2, l1); sub_v3_v3v3(h, p, l1); - return(dot_v3v3(u,h)/dot_v3v3(u,u)); + return (dot_v3v3(u, h) / dot_v3v3(u, u)); } float line_point_factor_v2(const float p[2], const float l1[2], const float l2[2]) @@ -1320,7 +1324,7 @@ float line_point_factor_v2(const float p[2], const float l1[2], const float l2[2 float h[2], u[2]; sub_v2_v2v2(u, l2, l1); sub_v2_v2v2(h, p, l1); - return(dot_v2v2(u, h)/dot_v2v2(u, u)); + return (dot_v2v2(u, h) / dot_v2v2(u, u)); } /* ensyre the distance between these points is no greater then 'dist' @@ -1343,32 +1347,33 @@ void limit_dist_v3(float v1[3], float v2[3], const float dist) } /* Similar to LineIntersectsTriangleUV, except it operates on a quad and in 2d, assumes point is in quad */ -void isect_point_quad_uv_v2(const float v0[2], const float v1[2], const float v2[2], const float v3[2], const float pt[2], float r_uv[2]) +void isect_point_quad_uv_v2(const float v0[2], const float v1[2], const float v2[2], const float v3[2], + const float pt[2], float r_uv[2]) { - float x0,y0, x1,y1, wtot, v2d[2], w1, w2; - + float x0, y0, x1, y1, wtot, v2d[2], w1, w2; + /* used for parallel lines */ float pt3d[3], l1[3], l2[3], pt_on_line[3]; - + /* compute 2 edges of the quad intersection point */ - if (IsectLLPt2Df(v0[0],v0[1],v1[0],v1[1], v2[0],v2[1],v3[0],v3[1], &x0,&y0) == 1) { + if (IsectLLPt2Df(v0[0], v0[1], v1[0], v1[1], v2[0], v2[1], v3[0], v3[1], &x0, &y0) == 1) { /* the intersection point between the quad-edge intersection and the point in the quad we want the uv's for */ /* should never be paralle !! */ /*printf("\tnot parallel 1\n");*/ - IsectLLPt2Df(pt[0],pt[1],x0,y0, v0[0],v0[1],v3[0],v3[1], &x1,&y1); - + IsectLLPt2Df(pt[0], pt[1], x0, y0, v0[0], v0[1], v3[0], v3[1], &x1, &y1); + /* Get the weights from the new intersection point, to each edge */ - v2d[0] = x1-v0[0]; - v2d[1] = y1-v0[1]; + v2d[0] = x1 - v0[0]; + v2d[1] = y1 - v0[1]; w1 = len_v2(v2d); - - v2d[0] = x1-v3[0]; /* some but for the other vert */ - v2d[1] = y1-v3[1]; + + v2d[0] = x1 - v3[0]; /* some but for the other vert */ + v2d[1] = y1 - v3[1]; w2 = len_v2(v2d); - wtot = w1+w2; + wtot = w1 + w2; /*w1 = w1/wtot;*/ /*w2 = w2/wtot;*/ - r_uv[0] = w1/wtot; + r_uv[0] = w1 / wtot; } else { /* lines are parallel, lambda_cp_line_ex is 3d grrr */ @@ -1376,40 +1381,44 @@ void isect_point_quad_uv_v2(const float v0[2], const float v1[2], const float v2 pt3d[0] = pt[0]; pt3d[1] = pt[1]; pt3d[2] = l1[2] = l2[2] = 0.0f; - - l1[0] = v0[0]; l1[1] = v0[1]; - l2[0] = v1[0]; l2[1] = v1[1]; - closest_to_line_v3(pt_on_line,pt3d, l1, l2); - v2d[0] = pt[0]-pt_on_line[0]; /* same, for the other vert */ - v2d[1] = pt[1]-pt_on_line[1]; + + l1[0] = v0[0]; + l1[1] = v0[1]; + l2[0] = v1[0]; + l2[1] = v1[1]; + closest_to_line_v3(pt_on_line, pt3d, l1, l2); + v2d[0] = pt[0] - pt_on_line[0]; /* same, for the other vert */ + v2d[1] = pt[1] - pt_on_line[1]; w1 = len_v2(v2d); - - l1[0] = v2[0]; l1[1] = v2[1]; - l2[0] = v3[0]; l2[1] = v3[1]; - closest_to_line_v3(pt_on_line,pt3d, l1, l2); - v2d[0] = pt[0]-pt_on_line[0]; /* same, for the other vert */ - v2d[1] = pt[1]-pt_on_line[1]; + + l1[0] = v2[0]; + l1[1] = v2[1]; + l2[0] = v3[0]; + l2[1] = v3[1]; + closest_to_line_v3(pt_on_line, pt3d, l1, l2); + v2d[0] = pt[0] - pt_on_line[0]; /* same, for the other vert */ + v2d[1] = pt[1] - pt_on_line[1]; w2 = len_v2(v2d); - wtot = w1+w2; - r_uv[0] = w1/wtot; + wtot = w1 + w2; + r_uv[0] = w1 / wtot; } - + /* Same as above to calc the uv[1] value, alternate calculation */ - - if (IsectLLPt2Df(v0[0],v0[1],v3[0],v3[1], v1[0],v1[1],v2[0],v2[1], &x0,&y0) == 1) { /* was v0,v1 v2,v3 now v0,v3 v1,v2*/ + + if (IsectLLPt2Df(v0[0], v0[1], v3[0], v3[1], v1[0], v1[1], v2[0], v2[1], &x0, &y0) == 1) { /* was v0,v1 v2,v3 now v0,v3 v1,v2*/ /* never paralle if above was not */ /*printf("\tnot parallel2\n");*/ - IsectLLPt2Df(pt[0],pt[1],x0,y0, v0[0],v0[1],v1[0],v1[1], &x1,&y1);/* was v0,v3 now v0,v1*/ - - v2d[0] = x1-v0[0]; - v2d[1] = y1-v0[1]; + IsectLLPt2Df(pt[0], pt[1], x0, y0, v0[0], v0[1], v1[0], v1[1], &x1, &y1); /* was v0,v3 now v0,v1*/ + + v2d[0] = x1 - v0[0]; + v2d[1] = y1 - v0[1]; w1 = len_v2(v2d); - - v2d[0] = x1-v1[0]; - v2d[1] = y1-v1[1]; + + v2d[0] = x1 - v1[0]; + v2d[1] = y1 - v1[1]; w2 = len_v2(v2d); - wtot = w1+w2; - r_uv[1] = w1/wtot; + wtot = w1 + w2; + r_uv[1] = w1 / wtot; } else { /* lines are parallel, lambda_cp_line_ex is 3d grrr */ @@ -1417,29 +1426,35 @@ void isect_point_quad_uv_v2(const float v0[2], const float v1[2], const float v2 pt3d[0] = pt[0]; pt3d[1] = pt[1]; pt3d[2] = l1[2] = l2[2] = 0.0f; - - - l1[0] = v0[0]; l1[1] = v0[1]; - l2[0] = v3[0]; l2[1] = v3[1]; - closest_to_line_v3(pt_on_line,pt3d, l1, l2); - v2d[0] = pt[0]-pt_on_line[0]; /* some but for the other vert */ - v2d[1] = pt[1]-pt_on_line[1]; + + + l1[0] = v0[0]; + l1[1] = v0[1]; + l2[0] = v3[0]; + l2[1] = v3[1]; + closest_to_line_v3(pt_on_line, pt3d, l1, l2); + v2d[0] = pt[0] - pt_on_line[0]; /* some but for the other vert */ + v2d[1] = pt[1] - pt_on_line[1]; w1 = len_v2(v2d); - - l1[0] = v1[0]; l1[1] = v1[1]; - l2[0] = v2[0]; l2[1] = v2[1]; - closest_to_line_v3(pt_on_line,pt3d, l1, l2); - v2d[0] = pt[0]-pt_on_line[0]; /* some but for the other vert */ - v2d[1] = pt[1]-pt_on_line[1]; + + l1[0] = v1[0]; + l1[1] = v1[1]; + l2[0] = v2[0]; + l2[1] = v2[1]; + closest_to_line_v3(pt_on_line, pt3d, l1, l2); + v2d[0] = pt[0] - pt_on_line[0]; /* some but for the other vert */ + v2d[1] = pt[1] - pt_on_line[1]; w2 = len_v2(v2d); - wtot = w1+w2; - r_uv[1] = w1/wtot; + wtot = w1 + w2; + r_uv[1] = w1 / wtot; } /* may need to flip UV's here */ } /* same as above but does tri's and quads, tri's are a bit of a hack */ -void isect_point_face_uv_v2(const int isquad, const float v0[2], const float v1[2], const float v2[2], const float v3[2], const float pt[2], float r_uv[2]) +void isect_point_face_uv_v2(const int isquad, + const float v0[2], const float v1[2], const float v2[2], const float v3[2], + const float pt[2], float r_uv[2]) { if (isquad) { isect_point_quad_uv_v2(v0, v1, v2, v3, pt, r_uv); @@ -1447,72 +1462,74 @@ void isect_point_face_uv_v2(const int isquad, const float v0[2], const float v1[ else { /* not for quads, use for our abuse of LineIntersectsTriangleUV */ float p1_3d[3], p2_3d[3], v0_3d[3], v1_3d[3], v2_3d[3], lambda; - + p1_3d[0] = p2_3d[0] = r_uv[0]; p1_3d[1] = p2_3d[1] = r_uv[1]; p1_3d[2] = 1.0f; p2_3d[2] = -1.0f; v0_3d[2] = v1_3d[2] = v2_3d[2] = 0.0; - + /* generate a new fuv, (this is possibly a non optimal solution, * since we only need 2d calculation but use 3d func's) - * + * * this method makes an imaginary triangle in 2d space using the UV's from the derived mesh face * Then find new uv coords using the fuv and this face with LineIntersectsTriangleUV. - * This means the new values will be correct in relation to the derived meshes face. + * This means the new values will be correct in relation to the derived meshes face. */ copy_v2_v2(v0_3d, v0); copy_v2_v2(v1_3d, v1); copy_v2_v2(v2_3d, v2); - + /* Doing this in 3D is not nice */ isect_line_tri_v3(p1_3d, p2_3d, v0_3d, v1_3d, v2_3d, &lambda, r_uv); } } #if 0 // XXX this version used to be used in isect_point_tri_v2_int() and was called IsPointInTri2D + int isect_point_tri_v2(float pt[2], float v1[2], float v2[2], float v3[2]) { float inp1, inp2, inp3; - - inp1= (v2[0]-v1[0])*(v1[1]-pt[1]) + (v1[1]-v2[1])*(v1[0]-pt[0]); - inp2= (v3[0]-v2[0])*(v2[1]-pt[1]) + (v2[1]-v3[1])*(v2[0]-pt[0]); - inp3= (v1[0]-v3[0])*(v3[1]-pt[1]) + (v3[1]-v1[1])*(v3[0]-pt[0]); - - if (inp1<=0.0f && inp2<=0.0f && inp3<=0.0f) return 1; - if (inp1>=0.0f && inp2>=0.0f && inp3>=0.0f) return 1; - + + inp1 = (v2[0] - v1[0]) * (v1[1] - pt[1]) + (v1[1] - v2[1]) * (v1[0] - pt[0]); + inp2 = (v3[0] - v2[0]) * (v2[1] - pt[1]) + (v2[1] - v3[1]) * (v2[0] - pt[0]); + inp3 = (v1[0] - v3[0]) * (v3[1] - pt[1]) + (v3[1] - v1[1]) * (v3[0] - pt[0]); + + if (inp1 <= 0.0f && inp2 <= 0.0f && inp3 <= 0.0f) return 1; + if (inp1 >= 0.0f && inp2 >= 0.0f && inp3 >= 0.0f) return 1; + return 0; } #endif #if 0 + int isect_point_tri_v2(float v0[2], float v1[2], float v2[2], float pt[2]) { - /* not for quads, use for our abuse of LineIntersectsTriangleUV */ - float p1_3d[3], p2_3d[3], v0_3d[3], v1_3d[3], v2_3d[3]; - /* not used */ - float lambda, uv[3]; - - p1_3d[0] = p2_3d[0] = uv[0]= pt[0]; - p1_3d[1] = p2_3d[1] = uv[1]= uv[2]= pt[1]; - p1_3d[2] = 1.0f; - p2_3d[2] = -1.0f; - v0_3d[2] = v1_3d[2] = v2_3d[2] = 0.0; - - /* generate a new fuv, (this is possibly a non optimal solution, - * since we only need 2d calculation but use 3d func's) - * - * this method makes an imaginary triangle in 2d space using the UV's from the derived mesh face - * Then find new uv coords using the fuv and this face with LineIntersectsTriangleUV. - * This means the new values will be correct in relation to the derived meshes face. - */ - copy_v2_v2(v0_3d, v0); - copy_v2_v2(v1_3d, v1); - copy_v2_v2(v2_3d, v2); - - /* Doing this in 3D is not nice */ - return isect_line_tri_v3(p1_3d, p2_3d, v0_3d, v1_3d, v2_3d, &lambda, uv); + /* not for quads, use for our abuse of LineIntersectsTriangleUV */ + float p1_3d[3], p2_3d[3], v0_3d[3], v1_3d[3], v2_3d[3]; + /* not used */ + float lambda, uv[3]; + + p1_3d[0] = p2_3d[0] = uv[0] = pt[0]; + p1_3d[1] = p2_3d[1] = uv[1] = uv[2] = pt[1]; + p1_3d[2] = 1.0f; + p2_3d[2] = -1.0f; + v0_3d[2] = v1_3d[2] = v2_3d[2] = 0.0; + + /* generate a new fuv, (this is possibly a non optimal solution, + * since we only need 2d calculation but use 3d func's) + * + * this method makes an imaginary triangle in 2d space using the UV's from the derived mesh face + * Then find new uv coords using the fuv and this face with LineIntersectsTriangleUV. + * This means the new values will be correct in relation to the derived meshes face. + */ + copy_v2_v2(v0_3d, v0); + copy_v2_v2(v1_3d, v1); + copy_v2_v2(v2_3d, v2); + + /* Doing this in 3D is not nice */ + return isect_line_tri_v3(p1_3d, p2_3d, v0_3d, v1_3d, v2_3d, &lambda, uv); } #endif @@ -1526,19 +1543,19 @@ int isect_point_tri_v2(float v0[2], float v1[2], float v2[2], float pt[2]) int isect_point_tri_v2_int(const int x1, const int y1, const int x2, const int y2, const int a, const int b) { float v1[2], v2[2], v3[2], p[2]; - - v1[0]= (float)x1; - v1[1]= (float)y1; - - v2[0]= (float)x1; - v2[1]= (float)y2; - - v3[0]= (float)x2; - v3[1]= (float)y1; - - p[0]= (float)a; - p[1]= (float)b; - + + v1[0] = (float)x1; + v1[1] = (float)y1; + + v2[0] = (float)x1; + v2[1] = (float)y2; + + v3[0] = (float)x2; + v3[1] = (float)y1; + + p[0] = (float)a; + p[1] = (float)b; + return isect_point_tri_v2(p, v1, v2, v3); } @@ -1547,44 +1564,45 @@ static int point_in_slice(const float p[3], const float v1[3], const float l1[3] /* * what is a slice ? * some maths: - * a line including l1,l2 and a point not on the line + * a line including l1,l2 and a point not on the line * define a subset of R3 delimited by planes parallel to the line and orthogonal - * to the (point --> line) distance vector,one plane on the line one on the point, + * to the (point --> line) distance vector,one plane on the line one on the point, * the room inside usually is rather small compared to R3 though still infinte - * useful for restricting (speeding up) searches + * useful for restricting (speeding up) searches * e.g. all points of triangular prism are within the intersection of 3 'slices' - * onother trivial case : cube + * onother trivial case : cube * but see a 'spat' which is a deformed cube with paired parallel planes needs only 3 slices too */ - float h,rp[3],cp[3],q[3]; + float h, rp[3], cp[3], q[3]; - closest_to_line_v3(cp,v1,l1,l2); - sub_v3_v3v3(q,cp,v1); + closest_to_line_v3(cp, v1, l1, l2); + sub_v3_v3v3(q, cp, v1); - sub_v3_v3v3(rp,p,v1); - h=dot_v3v3(q,rp)/dot_v3v3(q,q); + sub_v3_v3v3(rp, p, v1); + h = dot_v3v3(q, rp) / dot_v3v3(q, q); if (h < 0.0f || h > 1.0f) return 0; return 1; } #if 0 + /* adult sister defining the slice planes by the origin and the normal * NOTE |normal| may not be 1 but defining the thickness of the slice */ -static int point_in_slice_as(float p[3],float origin[3],float normal[3]) +static int point_in_slice_as(float p[3], float origin[3], float normal[3]) { - float h,rp[3]; - sub_v3_v3v3(rp,p,origin); - h=dot_v3v3(normal,rp)/dot_v3v3(normal,normal); + float h, rp[3]; + sub_v3_v3v3(rp, p, origin); + h = dot_v3v3(normal, rp) / dot_v3v3(normal, normal); if (h < 0.0f || h > 1.0f) return 0; return 1; } /*mama (knowing the squared length of the normal)*/ -static int point_in_slice_m(float p[3],float origin[3],float normal[3],float lns) +static int point_in_slice_m(float p[3], float origin[3], float normal[3], float lns) { - float h,rp[3]; - sub_v3_v3v3(rp,p,origin); - h=dot_v3v3(normal,rp)/lns; + float h, rp[3]; + sub_v3_v3v3(rp, p, origin); + h = dot_v3v3(normal, rp) / lns; if (h < 0.0f || h > 1.0f) return 0; return 1; } @@ -1592,9 +1610,9 @@ static int point_in_slice_m(float p[3],float origin[3],float normal[3],float lns int isect_point_tri_prism_v3(const float p[3], const float v1[3], const float v2[3], const float v3[3]) { - if (!point_in_slice(p,v1,v2,v3)) return 0; - if (!point_in_slice(p,v2,v3,v1)) return 0; - if (!point_in_slice(p,v3,v1,v2)) return 0; + if (!point_in_slice(p, v1, v2, v3)) return 0; + if (!point_in_slice(p, v2, v3, v1)) return 0; + if (!point_in_slice(p, v3, v1, v2)) return 0; return 1; } @@ -1604,12 +1622,12 @@ int clip_line_plane(float p1[3], float p2[3], const float plane[4]) copy_v3_v3(n, plane); sub_v3_v3v3(dp, p2, p1); - div= dot_v3v3(dp, n); + div = dot_v3v3(dp, n); if (div == 0.0f) /* parallel */ return 1; - t= -(dot_v3v3(p1, n) + plane[3])/div; + t = -(dot_v3v3(p1, n) + plane[3]) / div; if (div > 0.0f) { /* behind plane, completely clipped */ @@ -1647,20 +1665,19 @@ int clip_line_plane(float p1[3], float p2[3], const float plane[4]) } } - void plot_line_v2v2i(const int p1[2], const int p2[2], int (*callback)(int, int, void *), void *userData) { - int x1= p1[0]; - int y1= p1[1]; - int x2= p2[0]; - int y2= p2[1]; + int x1 = p1[0]; + int y1 = p1[1]; + int x2 = p2[0]; + int y2 = p2[1]; signed char ix; signed char iy; // if x1 == x2 or y1 == y2, then it does not matter what we set here - int delta_x = (x2 > x1?(ix = 1, x2 - x1):(ix = -1, x1 - x2)) << 1; - int delta_y = (y2 > y1?(iy = 1, y2 - y1):(iy = -1, y1 - y2)) << 1; + int delta_x = (x2 > x1 ? (ix = 1, x2 - x1) : (ix = -1, x1 - x2)) << 1; + int delta_y = (y2 > y1 ? (iy = 1, y2 - y1) : (iy = -1, y1 - y2)) << 1; if (callback(x1, y1, userData) == 0) { return; @@ -1717,18 +1734,18 @@ void plot_line_v2v2i(const int p1[2], const int p2[2], int (*callback)(int, int, /* get the 2 dominant axis values, 0==X, 1==Y, 2==Z */ void axis_dominant_v3(int *axis_a, int *axis_b, const float axis[3]) { - const float xn= fabsf(axis[0]); - const float yn= fabsf(axis[1]); - const float zn= fabsf(axis[2]); + const float xn = fabsf(axis[0]); + const float yn = fabsf(axis[1]); + const float zn = fabsf(axis[2]); - if (zn >= xn && zn >= yn) { *axis_a= 0; *axis_b= 1; } - else if (yn >= xn && yn >= zn) { *axis_a= 0; *axis_b= 2; } - else { *axis_a= 1; *axis_b= 2; } + if (zn >= xn && zn >= yn) { *axis_a= 0; *axis_b = 1; } + else if (yn >= xn && yn >= zn) { *axis_a= 0; *axis_b = 2; } + else { *axis_a= 1; *axis_b = 2; } } static float tri_signed_area(const float v1[3], const float v2[3], const float v3[3], const int i, const int j) { - return 0.5f*((v1[i]-v2[i])*(v2[j]-v3[j]) + (v1[j]-v2[j])*(v3[i]-v2[i])); + return 0.5f * ((v1[i] - v2[i]) * (v2[j] - v3[j]) + (v1[j] - v2[j]) * (v3[i] - v2[i])); } /* return 1 when degenerate */ @@ -1760,17 +1777,17 @@ void interp_weights_face_v3(float w[4], const float v1[3], const float v2[3], co { float w2[3]; - w[0]= w[1]= w[2]= w[3]= 0.0f; + w[0] = w[1] = w[2] = w[3] = 0.0f; /* first check for exact match */ if (equals_v3v3(co, v1)) - w[0]= 1.0f; + w[0] = 1.0f; else if (equals_v3v3(co, v2)) - w[1]= 1.0f; + w[1] = 1.0f; else if (equals_v3v3(co, v3)) - w[2]= 1.0f; + w[2] = 1.0f; else if (v4 && equals_v3v3(co, v4)) - w[3]= 1.0f; + w[3] = 1.0f; else { /* otherwise compute barycentric interpolation weights */ float n1[3], n2[3], n[3]; @@ -1787,19 +1804,19 @@ void interp_weights_face_v3(float w[4], const float v1[3], const float v2[3], co /* OpenGL seems to split this way, so we do too */ if (v4) { - degenerate= barycentric_weights(v1, v2, v4, co, n, w); + degenerate = barycentric_weights(v1, v2, v4, co, n, w); SWAP(float, w[2], w[3]); if (degenerate || (w[0] < 0.0f)) { /* if w[1] is negative, co is on the other side of the v1-v3 edge, * so we interpolate using the other triangle */ - degenerate= barycentric_weights(v2, v3, v4, co, n, w2); + degenerate = barycentric_weights(v2, v3, v4, co, n, w2); if (!degenerate) { - w[0]= 0.0f; - w[1]= w2[0]; - w[2]= w2[1]; - w[3]= w2[2]; + w[0] = 0.0f; + w[1] = w2[0]; + w[2] = w2[1]; + w[3] = w2[2]; } } } @@ -1831,8 +1848,8 @@ void barycentric_weights_v2(const float v1[2], const float v2[2], const float v3 * calculate the location of a point in relation to the second triangle. * Useful for finding relative positions with geometry */ void barycentric_transform(float pt_tar[3], float const pt_src[3], - const float tri_tar_p1[3], const float tri_tar_p2[3], const float tri_tar_p3[3], - const float tri_src_p1[3], const float tri_src_p2[3], const float tri_src_p3[3]) + const float tri_tar_p1[3], const float tri_tar_p2[3], const float tri_tar_p3[3], + const float tri_src_p1[3], const float tri_src_p2[3], const float tri_src_p3[3]) { /* this works by moving the source triangle so its normal is pointing on the Z * axis where its barycentric wights can be calculated in 2D and its Z offset can @@ -1843,7 +1860,7 @@ void barycentric_transform(float pt_tar[3], float const pt_src[3], float no_tar[3], no_src[3]; float quat_src[4]; float pt_src_xy[3]; - float tri_xy_src[3][3]; + float tri_xy_src[3][3]; float w_src[3]; float area_tar, area_src; float z_ofs_src; @@ -1868,10 +1885,10 @@ void barycentric_transform(float pt_tar[3], float const pt_src[3], barycentric_weights_v2(tri_xy_src[0], tri_xy_src[1], tri_xy_src[2], pt_src_xy, w_src); interp_v3_v3v3v3(pt_tar, tri_tar_p1, tri_tar_p2, tri_tar_p3, w_src); - area_tar= sqrtf(area_tri_v3(tri_tar_p1, tri_tar_p2, tri_tar_p3)); - area_src= sqrtf(area_tri_v2(tri_xy_src[0], tri_xy_src[1], tri_xy_src[2])); + area_tar = sqrtf(area_tri_v3(tri_tar_p1, tri_tar_p2, tri_tar_p3)); + area_src = sqrtf(area_tri_v2(tri_xy_src[0], tri_xy_src[1], tri_xy_src[2])); - z_ofs_src= pt_src_xy[2] - tri_xy_src[0][2]; + z_ofs_src = pt_src_xy[2] - tri_xy_src[0][2]; madd_v3_v3v3fl(pt_tar, pt_tar, no_tar, (z_ofs_src / area_src) * area_tar); } @@ -1883,66 +1900,66 @@ int interp_sparse_array(float *array, int const list_size, const float skipval) int found_valid = 0; int i; - for (i=0; i < list_size; i++) { + for (i = 0; i < list_size; i++) { if (array[i] == skipval) - found_invalid= 1; + found_invalid = 1; else - found_valid= 1; + found_valid = 1; } - if (found_valid==0) { + if (found_valid == 0) { return -1; } - else if (found_invalid==0) { + else if (found_invalid == 0) { return 0; } else { /* found invalid depths, interpolate */ - float valid_last= skipval; - int valid_ofs= 0; + float valid_last = skipval; + int valid_ofs = 0; - float *array_up= MEM_callocN(sizeof(float) * list_size, "interp_sparse_array up"); - float *array_down= MEM_callocN(sizeof(float) * list_size, "interp_sparse_array up"); + float *array_up = MEM_callocN(sizeof(float)* list_size, "interp_sparse_array up"); + float *array_down = MEM_callocN(sizeof(float)* list_size, "interp_sparse_array up"); - int *ofs_tot_up= MEM_callocN(sizeof(int) * list_size, "interp_sparse_array tup"); - int *ofs_tot_down= MEM_callocN(sizeof(int) * list_size, "interp_sparse_array tdown"); + int *ofs_tot_up = MEM_callocN(sizeof(int)* list_size, "interp_sparse_array tup"); + int *ofs_tot_down = MEM_callocN(sizeof(int)* list_size, "interp_sparse_array tdown"); - for (i=0; i < list_size; i++) { + for (i = 0; i < list_size; i++) { if (array[i] == skipval) { - array_up[i]= valid_last; - ofs_tot_up[i]= ++valid_ofs; + array_up[i] = valid_last; + ofs_tot_up[i] = ++valid_ofs; } else { - valid_last= array[i]; - valid_ofs= 0; + valid_last = array[i]; + valid_ofs = 0; } } - valid_last= skipval; - valid_ofs= 0; + valid_last = skipval; + valid_ofs = 0; - for (i=list_size-1; i >= 0; i--) { + for (i = list_size - 1; i >= 0; i--) { if (array[i] == skipval) { - array_down[i]= valid_last; - ofs_tot_down[i]= ++valid_ofs; + array_down[i] = valid_last; + ofs_tot_down[i] = ++valid_ofs; } else { - valid_last= array[i]; - valid_ofs= 0; + valid_last = array[i]; + valid_ofs = 0; } } /* now blend */ - for (i=0; i < list_size; i++) { + for (i = 0; i < list_size; i++) { if (array[i] == skipval) { if (array_up[i] != skipval && array_down[i] != skipval) { - array[i]= ((array_up[i] * ofs_tot_down[i]) + (array_down[i] * ofs_tot_up[i])) / (float)(ofs_tot_down[i] + ofs_tot_up[i]); + array[i] = ((array_up[i] * ofs_tot_down[i]) + (array_down[i] * ofs_tot_up[i])) / (float)(ofs_tot_down[i] + ofs_tot_up[i]); } else if (array_up[i] != skipval) { - array[i]= array_up[i]; + array[i] = array_up[i]; } else if (array_down[i] != skipval) { - array[i]= array_down[i]; + array[i] = array_down[i]; } } } @@ -1967,14 +1984,14 @@ static float mean_value_half_tan(const float v1[3], const float v2[3], const flo sub_v3_v3v3(d3, v3, v1); cross_v3_v3v3(cross, d2, d3); - area= len_v3(cross); - dot= dot_v3v3(d2, d3); - len= len_v3(d2)*len_v3(d3); + area = len_v3(cross); + dot = dot_v3v3(d2, d3); + len = len_v3(d2) * len_v3(d3); if (area == 0.0f) return 0.0f; else - return (len - dot)/area; + return (len - dot) / area; } void interp_weights_poly_v3(float *w, float v[][3], const int n, const float co[3]) @@ -1982,49 +1999,49 @@ void interp_weights_poly_v3(float *w, float v[][3], const int n, const float co[ float totweight, t1, t2, len, *vmid, *vprev, *vnext; int i; - totweight= 0.0f; + totweight = 0.0f; - for (i=0; i<n; i++) { - vmid= v[i]; - vprev= (i == 0)? v[n-1]: v[i-1]; - vnext= (i == n-1)? v[0]: v[i+1]; + for (i = 0; i < n; i++) { + vmid = v[i]; + vprev = (i == 0) ? v[n - 1] : v[i - 1]; + vnext = (i == n - 1) ? v[0] : v[i + 1]; - t1= mean_value_half_tan(co, vprev, vmid); - t2= mean_value_half_tan(co, vmid, vnext); + t1 = mean_value_half_tan(co, vprev, vmid); + t2 = mean_value_half_tan(co, vmid, vnext); - len= len_v3v3(co, vmid); - w[i]= (t1+t2)/len; + len = len_v3v3(co, vmid); + w[i] = (t1 + t2) / len; totweight += w[i]; } if (totweight != 0.0f) - for (i=0; i<n; i++) + for (i = 0; i < n; i++) w[i] /= totweight; } /* (x1,v1)(t1=0)------(x2,v2)(t2=1), 0<t<1 --> (x,v)(t) */ void interp_cubic_v3(float x[3], float v[3], const float x1[3], const float v1[3], const float x2[3], const float v2[3], const float t) { - float a[3],b[3]; - float t2= t*t; - float t3= t2*t; + float a[3], b[3]; + float t2 = t * t; + float t3 = t2 * t; /* cubic interpolation */ - a[0]= v1[0] + v2[0] + 2*(x1[0] - x2[0]); - a[1]= v1[1] + v2[1] + 2*(x1[1] - x2[1]); - a[2]= v1[2] + v2[2] + 2*(x1[2] - x2[2]); + a[0] = v1[0] + v2[0] + 2 * (x1[0] - x2[0]); + a[1] = v1[1] + v2[1] + 2 * (x1[1] - x2[1]); + a[2] = v1[2] + v2[2] + 2 * (x1[2] - x2[2]); - b[0]= -2*v1[0] - v2[0] - 3*(x1[0] - x2[0]); - b[1]= -2*v1[1] - v2[1] - 3*(x1[1] - x2[1]); - b[2]= -2*v1[2] - v2[2] - 3*(x1[2] - x2[2]); + b[0] = -2 * v1[0] - v2[0] - 3 * (x1[0] - x2[0]); + b[1] = -2 * v1[1] - v2[1] - 3 * (x1[1] - x2[1]); + b[2] = -2 * v1[2] - v2[2] - 3 * (x1[2] - x2[2]); - x[0]= a[0]*t3 + b[0]*t2 + v1[0]*t + x1[0]; - x[1]= a[1]*t3 + b[1]*t2 + v1[1]*t + x1[1]; - x[2]= a[2]*t3 + b[2]*t2 + v1[2]*t + x1[2]; + x[0] = a[0] * t3 + b[0] * t2 + v1[0] * t + x1[0]; + x[1] = a[1] * t3 + b[1] * t2 + v1[1] * t + x1[1]; + x[2] = a[2] * t3 + b[2] * t2 + v1[2] * t + x1[2]; - v[0]= 3*a[0]*t2 + 2*b[0]*t + v1[0]; - v[1]= 3*a[1]*t2 + 2*b[1]*t + v1[1]; - v[2]= 3*a[2]*t2 + 2*b[2]*t + v1[2]; + v[0] = 3 * a[0] * t2 + 2 * b[0] * t + v1[0]; + v[1] = 3 * a[1] * t2 + 2 * b[1] * t + v1[1]; + v[2] = 3 * a[2] * t2 + 2 * b[2] * t + v1[2]; } /* unfortunately internal calculations have to be done at double precision to achieve correct/stable results. */ @@ -2035,17 +2052,17 @@ void interp_cubic_v3(float x[3], float v[3], const float x1[3], const float v1[3 void resolve_tri_uv(float r_uv[2], const float st[2], const float st0[2], const float st1[2], const float st2[2]) { /* find UV such that - * t= u*t0 + v*t1 + (1-u-v)*t2 - * u*(t0-t2) + v*(t1-t2)= t-t2 */ - const double a= st0[0]-st2[0], b= st1[0]-st2[0]; - const double c= st0[1]-st2[1], d= st1[1]-st2[1]; - const double det= a*d - c*b; + * t = u * t0 + v * t1 + (1 - u - v) * t2 + * u * (t0 - t2) + v * (t1 - t2) = t - t2 */ + const double a = st0[0] - st2[0], b = st1[0] - st2[0]; + const double c = st0[1] - st2[1], d = st1[1] - st2[1]; + const double det = a * d - c * b; - if (IS_ZERO(det)==0) { /* det should never be zero since the determinant is the signed ST area of the triangle. */ - const double x[]= {st[0]-st2[0], st[1]-st2[1]}; + if (IS_ZERO(det) == 0) { /* det should never be zero since the determinant is the signed ST area of the triangle. */ + const double x[] = {st[0] - st2[0], st[1] - st2[1]}; - r_uv[0]= (float)((d*x[0] - b*x[1])/det); - r_uv[1]= (float)(((-c)*x[0] + a*x[1])/det); + r_uv[0] = (float)((d * x[0] - b * x[1]) / det); + r_uv[1] = (float)(((-c) * x[0] + a * x[1]) / det); } else zero_v2(r_uv); } @@ -2053,51 +2070,52 @@ void resolve_tri_uv(float r_uv[2], const float st[2], const float st0[2], const /* bilinear reverse */ void resolve_quad_uv(float r_uv[2], const float st[2], const float st0[2], const float st1[2], const float st2[2], const float st3[2]) { - const double signed_area= (st0[0]*st1[1] - st0[1]*st1[0]) + (st1[0]*st2[1] - st1[1]*st2[0]) + - (st2[0]*st3[1] - st2[1]*st3[0]) + (st3[0]*st0[1] - st3[1]*st0[0]); + const double signed_area = (st0[0] * st1[1] - st0[1] * st1[0]) + (st1[0] * st2[1] - st1[1] * st2[0]) + + (st2[0] * st3[1] - st2[1] * st3[0]) + (st3[0] * st0[1] - st3[1] * st0[0]); /* X is 2D cross product (determinant) * A= (p0-p) X (p0-p3)*/ - const double a= (st0[0]-st[0])*(st0[1]-st3[1]) - (st0[1]-st[1])*(st0[0]-st3[0]); + const double a = (st0[0] - st[0]) * (st0[1] - st3[1]) - (st0[1] - st[1]) * (st0[0] - st3[0]); /* B= ( (p0-p) X (p1-p2) + (p1-p) X (p0-p3) ) / 2 */ - const double b= 0.5 * ( ((st0[0]-st[0])*(st1[1]-st2[1]) - (st0[1]-st[1])*(st1[0]-st2[0])) + - ((st1[0]-st[0])*(st0[1]-st3[1]) - (st1[1]-st[1])*(st0[0]-st3[0])) ); + const double b = 0.5 * (((st0[0] - st[0]) * (st1[1] - st2[1]) - (st0[1] - st[1]) * (st1[0] - st2[0])) + + ((st1[0] - st[0]) * (st0[1] - st3[1]) - (st1[1] - st[1]) * (st0[0] - st3[0]))); /* C = (p1-p) X (p1-p2) */ - const double fC= (st1[0]-st[0])*(st1[1]-st2[1]) - (st1[1]-st[1])*(st1[0]-st2[0]); - const double denom= a - 2*b + fC; + const double fC = (st1[0] - st[0]) * (st1[1] - st2[1]) - (st1[1] - st[1]) * (st1[0] - st2[0]); + const double denom = a - 2 * b + fC; // clear outputs zero_v2(r_uv); - if (IS_ZERO(denom)!=0) { - const double fDen= a-fC; - if (IS_ZERO(fDen)==0) - r_uv[0]= (float)(a / fDen); + if (IS_ZERO(denom) != 0) { + const double fDen = a - fC; + if (IS_ZERO(fDen) == 0) + r_uv[0] = (float)(a / fDen); } else { - const double desc_sq= b*b - a*fC; - const double desc= sqrt(desc_sq<0.0?0.0:desc_sq); - const double s= signed_area>0 ? (-1.0) : 1.0; + const double desc_sq = b * b - a * fC; + const double desc = sqrt(desc_sq < 0.0 ? 0.0 : desc_sq); + const double s = signed_area > 0 ? (-1.0) : 1.0; - r_uv[0]= (float)(( (a-b) + s * desc ) / denom); + r_uv[0] = (float)(((a - b) + s * desc) / denom); } /* find UV such that - * fST = (1-u)(1-v)*ST0 + u*(1-v)*ST1 + u*v*ST2 + (1-u)*v*ST3 */ + * fST = (1-u)(1-v) * ST0 + u * (1-v) * ST1 + u * v * ST2 + (1-u) * v * ST3 */ { - const double denom_s= (1-r_uv[0])*(st0[0]-st3[0]) + r_uv[0]*(st1[0]-st2[0]); - const double denom_t= (1-r_uv[0])*(st0[1]-st3[1]) + r_uv[0]*(st1[1]-st2[1]); - int i= 0; double denom= denom_s; - - if (fabs(denom_s)<fabs(denom_t)) { - i= 1; - denom=denom_t; + const double denom_s = (1 - r_uv[0]) * (st0[0] - st3[0]) + r_uv[0] * (st1[0] - st2[0]); + const double denom_t = (1 - r_uv[0]) * (st0[1] - st3[1]) + r_uv[0] * (st1[1] - st2[1]); + int i = 0; + double denom = denom_s; + + if (fabs(denom_s) < fabs(denom_t)) { + i = 1; + denom = denom_t; } - if (IS_ZERO(denom)==0) - r_uv[1]= (float) (( (1.0f-r_uv[0])*(st0[i]-st[i]) + r_uv[0]*(st1[i]-st[i]) ) / denom); + if (IS_ZERO(denom) == 0) + r_uv[1] = (float)(((1.0f - r_uv[0]) * (st0[i] - st[i]) + r_uv[0] * (st1[i] - st[i])) / denom); } } @@ -2108,7 +2126,7 @@ void resolve_quad_uv(float r_uv[2], const float st[2], const float st0[2], const void orthographic_m4(float matrix[][4], const float left, const float right, const float bottom, const float top, const float nearClip, const float farClip) { float Xdelta, Ydelta, Zdelta; - + Xdelta = right - left; Ydelta = top - bottom; Zdelta = farClip - nearClip; @@ -2116,12 +2134,12 @@ void orthographic_m4(float matrix[][4], const float left, const float right, con return; } unit_m4(matrix); - matrix[0][0] = 2.0f/Xdelta; - matrix[3][0] = -(right + left)/Xdelta; - matrix[1][1] = 2.0f/Ydelta; - matrix[3][1] = -(top + bottom)/Ydelta; - matrix[2][2] = -2.0f/Zdelta; /* note: negate Z */ - matrix[3][2] = -(farClip + nearClip)/Zdelta; + matrix[0][0] = 2.0f / Xdelta; + matrix[3][0] = -(right + left) / Xdelta; + matrix[1][1] = 2.0f / Ydelta; + matrix[3][1] = -(top + bottom) / Ydelta; + matrix[2][2] = -2.0f / Zdelta; /* note: negate Z */ + matrix[3][2] = -(farClip + nearClip) / Zdelta; } void perspective_m4(float mat[4][4], const float left, const float right, const float bottom, const float top, const float nearClip, const float farClip) @@ -2135,16 +2153,16 @@ void perspective_m4(float mat[4][4], const float left, const float right, const if (Xdelta == 0.0f || Ydelta == 0.0f || Zdelta == 0.0f) { return; } - mat[0][0] = nearClip * 2.0f/Xdelta; - mat[1][1] = nearClip * 2.0f/Ydelta; - mat[2][0] = (right + left)/Xdelta; /* note: negate Z */ - mat[2][1] = (top + bottom)/Ydelta; - mat[2][2] = -(farClip + nearClip)/Zdelta; + mat[0][0] = nearClip * 2.0f / Xdelta; + mat[1][1] = nearClip * 2.0f / Ydelta; + mat[2][0] = (right + left) / Xdelta; /* note: negate Z */ + mat[2][1] = (top + bottom) / Ydelta; + mat[2][2] = -(farClip + nearClip) / Zdelta; mat[2][3] = -1.0f; - mat[3][2] = (-2.0f * nearClip * farClip)/Zdelta; + mat[3][2] = (-2.0f * nearClip * farClip) / Zdelta; mat[0][1] = mat[0][2] = mat[0][3] = - mat[1][0] = mat[1][2] = mat[1][3] = - mat[3][0] = mat[3][1] = mat[3][3] = 0.0; + mat[1][0] = mat[1][2] = mat[1][3] = + mat[3][0] = mat[3][1] = mat[3][3] = 0.0; } @@ -2157,16 +2175,16 @@ void window_translate_m4(float winmat[][4], float perspmat[][4], const float x, float v2[3]; float len1, len2; - v1[0]= perspmat[0][0]; - v1[1]= perspmat[1][0]; - v1[2]= perspmat[2][0]; + v1[0] = perspmat[0][0]; + v1[1] = perspmat[1][0]; + v1[2] = perspmat[2][0]; - v2[0]= perspmat[0][1]; - v2[1]= perspmat[1][1]; - v2[2]= perspmat[2][1]; + v2[0] = perspmat[0][1]; + v2[1] = perspmat[1][1]; + v2[2] = perspmat[2][1]; - len1= (1.0f / len_v3(v1)); - len2= (1.0f / len_v3(v2)); + len1 = (1.0f / len_v3(v1)); + len2 = (1.0f / len_v3(v2)); winmat[2][0] += len1 * winmat[0][0] * x; winmat[2][1] += len2 * winmat[1][1] * y; @@ -2182,32 +2200,31 @@ static void i_multmatrix(float icand[][4], float Vm[][4]) int row, col; float temp[4][4]; - for (row=0 ; row<4 ; row++) - for (col=0 ; col<4 ; col++) - temp[row][col] = icand[row][0] * Vm[0][col] - + icand[row][1] * Vm[1][col] - + icand[row][2] * Vm[2][col] - + icand[row][3] * Vm[3][col]; + for (row = 0; row < 4; row++) + for (col = 0; col < 4; col++) + temp[row][col] = (icand[row][0] * Vm[0][col] + + icand[row][1] * Vm[1][col] + + icand[row][2] * Vm[2][col] + + icand[row][3] * Vm[3][col]); copy_m4_m4(Vm, temp); } - -void polarview_m4(float Vm[][4],float dist, float azimuth, float incidence, float twist) +void polarview_m4(float Vm[][4], float dist, float azimuth, float incidence, float twist) { unit_m4(Vm); - translate_m4(Vm,0.0, 0.0, -dist); - rotate_m4(Vm,'Z',-twist); - rotate_m4(Vm,'X',-incidence); - rotate_m4(Vm,'Z',-azimuth); + translate_m4(Vm, 0.0, 0.0, -dist); + rotate_m4(Vm, 'Z', -twist); + rotate_m4(Vm, 'X', -incidence); + rotate_m4(Vm, 'Z', -azimuth); } -void lookat_m4(float mat[][4],float vx, float vy, float vz, float px, float py, float pz, float twist) +void lookat_m4(float mat[][4], float vx, float vy, float vz, float px, float py, float pz, float twist) { float sine, cosine, hyp, hyp1, dx, dy, dz; - float mat1[4][4]= MAT4_UNITY; - + float mat1[4][4] = MAT4_UNITY; + unit_m4(mat); rotate_m4(mat, 'Z', -twist); @@ -2215,13 +2232,13 @@ void lookat_m4(float mat[][4],float vx, float vy, float vz, float px, float py, dx = px - vx; dy = py - vy; dz = pz - vz; - hyp = dx * dx + dz * dz; /* hyp squared */ - hyp1 = (float)sqrt(dy*dy + hyp); - hyp = (float)sqrt(hyp); /* the real hyp */ - - if (hyp1 != 0.0f) { /* rotate X */ + hyp = dx * dx + dz * dz; /* hyp squared */ + hyp1 = (float)sqrt(dy * dy + hyp); + hyp = (float)sqrt(hyp); /* the real hyp */ + + if (hyp1 != 0.0f) { /* rotate X */ sine = -dy / hyp1; - cosine = hyp /hyp1; + cosine = hyp / hyp1; } else { sine = 0; @@ -2231,14 +2248,14 @@ void lookat_m4(float mat[][4],float vx, float vy, float vz, float px, float py, mat1[1][2] = sine; mat1[2][1] = -sine; mat1[2][2] = cosine; - + i_multmatrix(mat1, mat); - mat1[1][1] = mat1[2][2] = 1.0f; /* be careful here to reinit */ - mat1[1][2] = mat1[2][1] = 0.0; /* those modified by the last */ - + mat1[1][1] = mat1[2][2] = 1.0f; /* be careful here to reinit */ + mat1[1][2] = mat1[2][1] = 0.0; /* those modified by the last */ + /* paragraph */ - if (hyp != 0.0f) { /* rotate Y */ + if (hyp != 0.0f) { /* rotate Y */ sine = dx / hyp; cosine = -dz / hyp; } @@ -2250,31 +2267,31 @@ void lookat_m4(float mat[][4],float vx, float vy, float vz, float px, float py, mat1[0][2] = -sine; mat1[2][0] = sine; mat1[2][2] = cosine; - + i_multmatrix(mat1, mat); - translate_m4(mat,-vx,-vy,-vz); /* translate viewpoint to origin */ + translate_m4(mat, -vx, -vy, -vz); /* translate viewpoint to origin */ } int box_clip_bounds_m4(float boundbox[2][3], const float bounds[4], float winmat[4][4]) { float mat[4][4], vec[4]; - int a, fl, flag= -1; + int a, fl, flag = -1; copy_m4_m4(mat, winmat); - for (a=0; a<8; a++) { - vec[0]= (a & 1)? boundbox[0][0]: boundbox[1][0]; - vec[1]= (a & 2)? boundbox[0][1]: boundbox[1][1]; - vec[2]= (a & 4)? boundbox[0][2]: boundbox[1][2]; - vec[3]= 1.0; + for (a = 0; a < 8; a++) { + vec[0] = (a & 1) ? boundbox[0][0] : boundbox[1][0]; + vec[1] = (a & 2) ? boundbox[0][1] : boundbox[1][1]; + vec[2] = (a & 4) ? boundbox[0][2] : boundbox[1][2]; + vec[3] = 1.0; mul_m4_v4(mat, vec); - fl= 0; + fl = 0; if (bounds) { - if (vec[0] > bounds[1]*vec[3]) fl |= 1; - if (vec[0]< bounds[0]*vec[3]) fl |= 2; - if (vec[1] > bounds[3]*vec[3]) fl |= 4; - if (vec[1]< bounds[2]*vec[3]) fl |= 8; + if (vec[0] > bounds[1] * vec[3]) fl |= 1; + if (vec[0] < bounds[0] * vec[3]) fl |= 2; + if (vec[1] > bounds[3] * vec[3]) fl |= 4; + if (vec[1] < bounds[2] * vec[3]) fl |= 8; } else { if (vec[0] < -vec[3]) fl |= 1; @@ -2286,7 +2303,7 @@ int box_clip_bounds_m4(float boundbox[2][3], const float bounds[4], float winmat if (vec[2] > vec[3]) fl |= 32; flag &= fl; - if (flag==0) return 0; + if (flag == 0) return 0; } return flag; @@ -2300,10 +2317,10 @@ void box_minmax_bounds_m4(float min[3], float max[3], float boundbox[2][3], floa copy_v3_v3(mn, min); copy_v3_v3(mx, max); - for (a=0; a<8; a++) { - vec[0]= (a & 1)? boundbox[0][0]: boundbox[1][0]; - vec[1]= (a & 2)? boundbox[0][1]: boundbox[1][1]; - vec[2]= (a & 4)? boundbox[0][2]: boundbox[1][2]; + for (a = 0; a < 8; a++) { + vec[0] = (a & 1) ? boundbox[0][0] : boundbox[1][0]; + vec[1] = (a & 2) ? boundbox[0][1] : boundbox[1][1]; + vec[2] = (a & 4) ? boundbox[0][2] : boundbox[1][2]; mul_m4_v3(mat, vec); DO_MINMAX(vec, mn, mx); @@ -2318,12 +2335,12 @@ void box_minmax_bounds_m4(float min[3], float max[3], float boundbox[2][3], floa void map_to_tube(float *r_u, float *r_v, const float x, const float y, const float z) { float len; - + *r_v = (z + 1.0f) / 2.0f; - + len = sqrtf(x * x + y * y); if (len > 0.0f) { - *r_u = (float)((1.0 - (atan2(x/len,y/len) / M_PI)) / 2.0); + *r_u = (float)((1.0 - (atan2(x / len, y / len) / M_PI)) / 2.0); } else { *r_v = *r_u = 0.0f; /* to avoid un-initialized variables */ @@ -2333,13 +2350,13 @@ void map_to_tube(float *r_u, float *r_v, const float x, const float y, const flo void map_to_sphere(float *r_u, float *r_v, const float x, const float y, const float z) { float len; - + len = sqrtf(x * x + y * y + z * z); if (len > 0.0f) { - if (x==0.0f && y==0.0f) *r_u= 0.0f; /* othwise domain error */ - else *r_u = (1.0f - atan2f(x,y) / (float)M_PI) / 2.0f; + if (x == 0.0f && y == 0.0f) *r_u = 0.0f; /* othwise domain error */ + else *r_u = (1.0f - atan2f(x, y) / (float)M_PI) / 2.0f; - *r_v = 1.0f - (float)saacos(z/len)/(float)M_PI; + *r_v = 1.0f - (float)saacos(z / len) / (float)M_PI; } else { *r_v = *r_u = 0.0f; /* to avoid un-initialized variables */ @@ -2349,17 +2366,17 @@ void map_to_sphere(float *r_u, float *r_v, const float x, const float y, const f /********************************* Normals **********************************/ void accumulate_vertex_normals(float n1[3], float n2[3], float n3[3], - float n4[3], const float f_no[3], const float co1[3], const float co2[3], - const float co3[3], const float co4[3]) + float n4[3], const float f_no[3], const float co1[3], const float co2[3], + const float co3[3], const float co4[3]) { float vdiffs[4][3]; - const int nverts= (n4!=NULL && co4!=NULL)? 4: 3; + const int nverts = (n4 != NULL && co4 != NULL) ? 4 : 3; /* compute normalized edge vectors */ sub_v3_v3v3(vdiffs[0], co2, co1); sub_v3_v3v3(vdiffs[1], co3, co2); - if (nverts==3) { + if (nverts == 3) { sub_v3_v3v3(vdiffs[2], co1, co3); } else { @@ -2374,13 +2391,13 @@ void accumulate_vertex_normals(float n1[3], float n2[3], float n3[3], /* accumulate angle weighted face normal */ { - float *vn[]= {n1, n2, n3, n4}; - const float *prev_edge = vdiffs[nverts-1]; + float *vn[] = {n1, n2, n3, n4}; + const float *prev_edge = vdiffs[nverts - 1]; int i; - for (i=0; i<nverts; i++) { - const float *cur_edge= vdiffs[i]; - const float fac= saacos(-dot_v3v3(cur_edge, prev_edge)); + for (i = 0; i < nverts; i++) { + const float *cur_edge = vdiffs[i]; + const float fac = saacos(-dot_v3v3(cur_edge, prev_edge)); // accumulate madd_v3_v3fl(vn[i], f_no, fac); @@ -2392,27 +2409,27 @@ void accumulate_vertex_normals(float n1[3], float n2[3], float n3[3], /* Add weighted face normal component into normals of the face vertices. * Caller must pass pre-allocated vdiffs of nverts length. */ void accumulate_vertex_normals_poly(float **vertnos, float polyno[3], - float **vertcos, float vdiffs[][3], int nverts) + float **vertcos, float vdiffs[][3], int nverts) { int i; /* calculate normalized edge directions for each edge in the poly */ for (i = 0; i < nverts; i++) { - sub_v3_v3v3(vdiffs[i], vertcos[(i+1) % nverts], vertcos[i]); + sub_v3_v3v3(vdiffs[i], vertcos[(i + 1) % nverts], vertcos[i]); normalize_v3(vdiffs[i]); } /* accumulate angle weighted face normal */ { - const float *prev_edge = vdiffs[nverts-1]; + const float *prev_edge = vdiffs[nverts - 1]; int i; - for (i=0; i<nverts; i++) { + for (i = 0; i < nverts; i++) { const float *cur_edge = vdiffs[i]; /* calculate angle between the two poly edges incident on * this vertex */ - const float fac= saacos(-dot_v3v3(cur_edge, prev_edge)); + const float fac = saacos(-dot_v3v3(cur_edge, prev_edge)); /* accumulate */ madd_v3_v3fl(vertnos[i], polyno, fac); @@ -2424,7 +2441,7 @@ void accumulate_vertex_normals_poly(float **vertnos, float polyno[3], /********************************* Tangents **********************************/ /* For normal map tangents we need to detect uv boundaries, and only average - * tangents in case the uvs are connected. Alternative would be to store 1 + * tangents in case the uvs are connected. Alternative would be to store 1 * tangent per face rather than 4 per face vertex, but that's not compatible * with games */ @@ -2437,22 +2454,22 @@ void sum_or_add_vertex_tangent(void *arena, VertexTangent **vtang, const float t VertexTangent *vt; /* find a tangent with connected uvs */ - for (vt= *vtang; vt; vt=vt->next) { - if (fabsf(uv[0]-vt->uv[0]) < STD_UV_CONNECT_LIMIT && fabsf(uv[1]-vt->uv[1]) < STD_UV_CONNECT_LIMIT) { + for (vt = *vtang; vt; vt = vt->next) { + if (fabsf(uv[0] - vt->uv[0]) < STD_UV_CONNECT_LIMIT && fabsf(uv[1] - vt->uv[1]) < STD_UV_CONNECT_LIMIT) { add_v3_v3(vt->tang, tang); return; } } /* if not found, append a new one */ - vt= BLI_memarena_alloc((MemArena *)arena, sizeof(VertexTangent)); + vt = BLI_memarena_alloc((MemArena *) arena, sizeof(VertexTangent)); copy_v3_v3(vt->tang, tang); - vt->uv[0]= uv[0]; - vt->uv[1]= uv[1]; + vt->uv[0] = uv[0]; + vt->uv[1] = uv[1]; if (*vtang) - vt->next= *vtang; - *vtang= vt; + vt->next = *vtang; + *vtang = vt; } float *find_vertex_tangent(VertexTangent *vtang, const float uv[2]) @@ -2460,44 +2477,44 @@ float *find_vertex_tangent(VertexTangent *vtang, const float uv[2]) VertexTangent *vt; static float nulltang[3] = {0.0f, 0.0f, 0.0f}; - for (vt= vtang; vt; vt=vt->next) - if (fabsf(uv[0]-vt->uv[0]) < STD_UV_CONNECT_LIMIT && fabsf(uv[1]-vt->uv[1]) < STD_UV_CONNECT_LIMIT) + for (vt = vtang; vt; vt = vt->next) + if (fabsf(uv[0] - vt->uv[0]) < STD_UV_CONNECT_LIMIT && fabsf(uv[1] - vt->uv[1]) < STD_UV_CONNECT_LIMIT) return vt->tang; - return nulltang; /* shouldn't happen, except for nan or so */ + return nulltang; /* shouldn't happen, except for nan or so */ } void tangent_from_uv(float uv1[2], float uv2[2], float uv3[3], float co1[3], float co2[3], float co3[3], float n[3], float tang[3]) { - float s1= uv2[0] - uv1[0]; - float s2= uv3[0] - uv1[0]; - float t1= uv2[1] - uv1[1]; - float t2= uv3[1] - uv1[1]; - float det= (s1 * t2 - s2 * t1); + float s1 = uv2[0] - uv1[0]; + float s2 = uv3[0] - uv1[0]; + float t1 = uv2[1] - uv1[1]; + float t2 = uv3[1] - uv1[1]; + float det = (s1 * t2 - s2 * t1); if (det != 0.0f) { /* otherwise 'tang' becomes nan */ float tangv[3], ct[3], e1[3], e2[3]; - det= 1.0f/det; + det = 1.0f / det; /* normals in render are inversed... */ sub_v3_v3v3(e1, co1, co2); sub_v3_v3v3(e2, co1, co3); - tang[0] = (t2*e1[0] - t1*e2[0])*det; - tang[1] = (t2*e1[1] - t1*e2[1])*det; - tang[2] = (t2*e1[2] - t1*e2[2])*det; - tangv[0] = (s1*e2[0] - s2*e1[0])*det; - tangv[1] = (s1*e2[1] - s2*e1[1])*det; - tangv[2] = (s1*e2[2] - s2*e1[2])*det; + tang[0] = (t2 * e1[0] - t1 * e2[0]) * det; + tang[1] = (t2 * e1[1] - t1 * e2[1]) * det; + tang[2] = (t2 * e1[2] - t1 * e2[2]) * det; + tangv[0] = (s1 * e2[0] - s2 * e1[0]) * det; + tangv[1] = (s1 * e2[1] - s2 * e1[1]) * det; + tangv[2] = (s1 * e2[2] - s2 * e1[2]) * det; cross_v3_v3v3(ct, tang, tangv); - + /* check flip */ if (dot_v3v3(ct, n) < 0.0f) { negate_v3(tang); } } else { - tang[0]= tang[1]= tang[2]= 0.0; + tang[0] = tang[1] = tang[2] = 0.0; } } @@ -2511,7 +2528,7 @@ void tangent_from_uv(float uv1[2], float uv2[2], float uv3[3], float co1[3], flo * ( * int list_size * 4 lists as pointer to array[list_size] - * 1. current pos array of 'new' positions + * 1. current pos array of 'new' positions * 2. current weight array of 'new'weights (may be NULL pointer if you have no weights ) * 3. reference rpos array of 'old' positions * 4. reference rweight array of 'old'weights (may be NULL pointer if you have no weights ) @@ -2531,19 +2548,18 @@ static float _det_m3(float m2[3][3]) { float det = 0.f; if (m2) { - det= m2[0][0]* (m2[1][1]*m2[2][2] - m2[1][2]*m2[2][1]) - -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]); + det = (m2[0][0] * (m2[1][1] * m2[2][2] - m2[1][2] * m2[2][1]) - + 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])); } return det; } - -void vcloud_estimate_transform(int list_size, float (*pos)[3], float *weight,float (*rpos)[3], float *rweight, - float lloc[3],float rloc[3],float lrot[3][3],float lscale[3][3]) +void vcloud_estimate_transform(int list_size, float (*pos)[3], float *weight, float (*rpos)[3], float *rweight, + float lloc[3], float rloc[3], float lrot[3][3], float lscale[3][3]) { - float accu_com[3]= {0.0f,0.0f,0.0f}, accu_rcom[3]= {0.0f,0.0f,0.0f}; - float accu_weight = 0.0f,accu_rweight = 0.0f,eps = 0.000001f; + float accu_com[3] = {0.0f, 0.0f, 0.0f}, accu_rcom[3] = {0.0f, 0.0f, 0.0f}; + float accu_weight = 0.0f, accu_rweight = 0.0f, eps = 0.000001f; int a; /* first set up a nice default response */ @@ -2554,22 +2570,22 @@ void vcloud_estimate_transform(int list_size, float (*pos)[3], float *weight,flo /* do com for both clouds */ if (pos && rpos && (list_size > 0)) { /* paranoya check */ /* do com for both clouds */ - for (a=0; a<list_size; a++) { + for (a = 0; a < list_size; a++) { if (weight) { float v[3]; - copy_v3_v3(v,pos[a]); - mul_v3_fl(v,weight[a]); + copy_v3_v3(v, pos[a]); + mul_v3_fl(v, weight[a]); add_v3_v3(accu_com, v); - accu_weight +=weight[a]; + accu_weight += weight[a]; } else add_v3_v3(accu_com, pos[a]); if (rweight) { float v[3]; - copy_v3_v3(v,rpos[a]); - mul_v3_fl(v,rweight[a]); + copy_v3_v3(v, rpos[a]); + mul_v3_fl(v, rweight[a]); add_v3_v3(accu_rcom, v); - accu_rweight +=rweight[a]; + accu_rweight += rweight[a]; } else add_v3_v3(accu_rcom, rpos[a]); @@ -2578,25 +2594,25 @@ void vcloud_estimate_transform(int list_size, float (*pos)[3], float *weight,flo accu_weight = accu_rweight = list_size; } - mul_v3_fl(accu_com,1.0f/accu_weight); - mul_v3_fl(accu_rcom,1.0f/accu_rweight); - if (lloc) copy_v3_v3(lloc,accu_com); - if (rloc) copy_v3_v3(rloc,accu_rcom); + mul_v3_fl(accu_com, 1.0f / accu_weight); + mul_v3_fl(accu_rcom, 1.0f / accu_rweight); + if (lloc) copy_v3_v3(lloc, accu_com); + if (rloc) copy_v3_v3(rloc, accu_rcom); if (lrot || lscale) { /* caller does not want rot nor scale, strange but legal */ /*so now do some reverse engeneering and see if we can split rotation from scale ->Polardecompose*/ /* build 'projection' matrix */ - float m[3][3],mr[3][3],q[3][3],qi[3][3]; - float va[3],vb[3],stunt[3]; - float odet,ndet; - int i=0,imax=15; + float m[3][3], mr[3][3], q[3][3], qi[3][3]; + float va[3], vb[3], stunt[3]; + float odet, ndet; + int i = 0, imax = 15; zero_m3(m); zero_m3(mr); /* build 'projection' matrix */ - for (a=0; a<list_size; a++) { - sub_v3_v3v3(va,rpos[a],accu_rcom); + for (a = 0; a < list_size; a++) { + sub_v3_v3v3(va, rpos[a], accu_rcom); /* mul_v3_fl(va,bp->mass); mass needs renormalzation here ?? */ - sub_v3_v3v3(vb,pos[a],accu_com); + sub_v3_v3v3(vb, pos[a], accu_com); /* mul_v3_fl(va,rp->mass); */ m[0][0] += va[0] * vb[0]; m[0][1] += va[0] * vb[1]; @@ -2625,20 +2641,22 @@ void vcloud_estimate_transform(int list_size, float (*pos)[3], float *weight,flo mr[2][1] += va[2] * va[1]; mr[2][2] += va[2] * va[2]; } - copy_m3_m3(q,m); - stunt[0] = q[0][0]; stunt[1] = q[1][1]; stunt[2] = q[2][2]; + copy_m3_m3(q, m); + stunt[0] = q[0][0]; + stunt[1] = q[1][1]; + stunt[2] = q[2][2]; /* renormalizing for numeric stability */ - mul_m3_fl(q,1.f/len_v3(stunt)); + mul_m3_fl(q, 1.f / len_v3(stunt)); /* this is pretty much Polardecompose 'inline' the algo based on Higham's thesis */ /* without the far case ... but seems to work here pretty neat */ odet = 0.f; ndet = _det_m3(q); - while ((odet-ndet)*(odet-ndet) > eps && i<imax) { - invert_m3_m3(qi,q); + while ((odet - ndet) * (odet - ndet) > eps && i < imax) { + invert_m3_m3(qi, q); transpose_m3(qi); - add_m3_m3m3(q,q,qi); - mul_m3_fl(q,0.5f); + add_m3_m3m3(q, q, qi); + mul_m3_fl(q, 0.5f); odet = ndet; ndet = _det_m3(q); i++; @@ -2647,12 +2665,12 @@ void vcloud_estimate_transform(int list_size, float (*pos)[3], float *weight,flo if (i) { float scale[3][3]; float irot[3][3]; - if (lrot) copy_m3_m3(lrot,q); - invert_m3_m3(irot,q); - invert_m3_m3(qi,mr); - mul_m3_m3m3(q,m,qi); - mul_m3_m3m3(scale,irot,q); - if (lscale) copy_m3_m3(lscale,scale); + if (lrot) copy_m3_m3(lrot, q); + invert_m3_m3(irot, q); + invert_m3_m3(qi, mr); + mul_m3_m3m3(q, m, qi); + mul_m3_m3m3(scale, irot, q); + if (lscale) copy_m3_m3(lscale, scale); } } @@ -2663,22 +2681,24 @@ void vcloud_estimate_transform(int list_size, float (*pos)[3], float *weight,flo static void vec_add_dir(float r[3], const float v1[3], const float v2[3], const float fac) { - r[0]= v1[0] + fac*(v2[0] - v1[0]); - r[1]= v1[1] + fac*(v2[1] - v1[1]); - r[2]= v1[2] + fac*(v2[2] - v1[2]); + r[0] = v1[0] + fac * (v2[0] - v1[0]); + r[1] = v1[1] + fac * (v2[1] - v1[1]); + r[2] = v1[2] + fac * (v2[2] - v1[2]); } -static int ff_visible_quad(const float p[3], const float n[3], const float v0[3], const float v1[3], const float v2[3], float q0[3], float q1[3], float q2[3], float q3[3]) +static int ff_visible_quad(const float p[3], const float n[3], + const float v0[3], const float v1[3], const float v2[3], + float q0[3], float q1[3], float q2[3], float q3[3]) { static const float epsilon = 1e-6f; float c, sd[3]; - - c= dot_v3v3(n, p); + + c = dot_v3v3(n, p); /* signed distances from the vertices to the plane. */ - sd[0]= dot_v3v3(n, v0) - c; - sd[1]= dot_v3v3(n, v1) - c; - sd[2]= dot_v3v3(n, v2) - c; + sd[0] = dot_v3v3(n, v0) - c; + sd[1] = dot_v3v3(n, v1) - c; + sd[2] = dot_v3v3(n, v2) - c; if (fabsf(sd[0]) < epsilon) sd[0] = 0.0f; if (fabsf(sd[1]) < epsilon) sd[1] = 0.0f; @@ -2697,8 +2717,8 @@ static int ff_visible_quad(const float p[3], const float n[3], const float v0[3] // ++- copy_v3_v3(q0, v0); copy_v3_v3(q1, v1); - vec_add_dir(q2, v1, v2, (sd[1]/(sd[1]-sd[2]))); - vec_add_dir(q3, v0, v2, (sd[0]/(sd[0]-sd[2]))); + vec_add_dir(q2, v1, v2, (sd[1] / (sd[1] - sd[2]))); + vec_add_dir(q3, v0, v2, (sd[0] / (sd[0] - sd[2]))); } else { // ++0 @@ -2712,21 +2732,21 @@ static int ff_visible_quad(const float p[3], const float n[3], const float v0[3] if (sd[2] > 0) { // +-+ copy_v3_v3(q0, v0); - vec_add_dir(q1, v0, v1, (sd[0]/(sd[0]-sd[1]))); - vec_add_dir(q2, v1, v2, (sd[1]/(sd[1]-sd[2]))); + vec_add_dir(q1, v0, v1, (sd[0] / (sd[0] - sd[1]))); + vec_add_dir(q2, v1, v2, (sd[1] / (sd[1] - sd[2]))); copy_v3_v3(q3, v2); } else if (sd[2] < 0) { // +-- copy_v3_v3(q0, v0); - vec_add_dir(q1, v0, v1, (sd[0]/(sd[0]-sd[1]))); - vec_add_dir(q2, v0, v2, (sd[0]/(sd[0]-sd[2]))); + vec_add_dir(q1, v0, v1, (sd[0] / (sd[0] - sd[1]))); + vec_add_dir(q2, v0, v2, (sd[0] / (sd[0] - sd[2]))); copy_v3_v3(q3, q2); } else { // +-0 copy_v3_v3(q0, v0); - vec_add_dir(q1, v0, v1, (sd[0]/(sd[0]-sd[1]))); + vec_add_dir(q1, v0, v1, (sd[0] / (sd[0] - sd[1]))); copy_v3_v3(q2, v2); copy_v3_v3(q3, q2); } @@ -2743,7 +2763,7 @@ static int ff_visible_quad(const float p[3], const float n[3], const float v0[3] // +0- copy_v3_v3(q0, v0); copy_v3_v3(q1, v1); - vec_add_dir(q2, v0, v2, (sd[0]/(sd[0]-sd[2]))); + vec_add_dir(q2, v0, v2, (sd[0] / (sd[0] - sd[2]))); copy_v3_v3(q3, q2); } else { @@ -2759,21 +2779,21 @@ static int ff_visible_quad(const float p[3], const float n[3], const float v0[3] if (sd[1] > 0) { if (sd[2] > 0) { // -++ - vec_add_dir(q0, v0, v1, (sd[0]/(sd[0]-sd[1]))); + vec_add_dir(q0, v0, v1, (sd[0] / (sd[0] - sd[1]))); copy_v3_v3(q1, v1); copy_v3_v3(q2, v2); - vec_add_dir(q3, v0, v2, (sd[0]/(sd[0]-sd[2]))); + vec_add_dir(q3, v0, v2, (sd[0] / (sd[0] - sd[2]))); } else if (sd[2] < 0) { // -+- - vec_add_dir(q0, v0, v1, (sd[0]/(sd[0]-sd[1]))); + vec_add_dir(q0, v0, v1, (sd[0] / (sd[0] - sd[1]))); copy_v3_v3(q1, v1); - vec_add_dir(q2, v1, v2, (sd[1]/(sd[1]-sd[2]))); + vec_add_dir(q2, v1, v2, (sd[1] / (sd[1] - sd[2]))); copy_v3_v3(q3, q2); } else { // -+0 - vec_add_dir(q0, v0, v1, (sd[0]/(sd[0]-sd[1]))); + vec_add_dir(q0, v0, v1, (sd[0] / (sd[0] - sd[1]))); copy_v3_v3(q1, v1); copy_v3_v3(q2, v2); copy_v3_v3(q3, q2); @@ -2782,8 +2802,8 @@ static int ff_visible_quad(const float p[3], const float n[3], const float v0[3] else if (sd[1] < 0) { if (sd[2] > 0) { // --+ - vec_add_dir(q0, v0, v2, (sd[0]/(sd[0]-sd[2]))); - vec_add_dir(q1, v1, v2, (sd[1]/(sd[1]-sd[2]))); + vec_add_dir(q0, v0, v2, (sd[0] / (sd[0] - sd[2]))); + vec_add_dir(q1, v1, v2, (sd[1] / (sd[1] - sd[2]))); copy_v3_v3(q2, v2); copy_v3_v3(q3, q2); } @@ -2799,7 +2819,7 @@ static int ff_visible_quad(const float p[3], const float n[3], const float v0[3] else { if (sd[2] > 0) { // -0+ - vec_add_dir(q0, v0, v2, (sd[0]/(sd[0]-sd[2]))); + vec_add_dir(q0, v0, v2, (sd[0] / (sd[0] - sd[2]))); copy_v3_v3(q1, v1); copy_v3_v3(q2, v2); copy_v3_v3(q3, q2); @@ -2827,7 +2847,7 @@ static int ff_visible_quad(const float p[3], const float n[3], const float v0[3] // 0+- copy_v3_v3(q0, v0); copy_v3_v3(q1, v1); - vec_add_dir(q2, v1, v2, (sd[1]/(sd[1]-sd[2]))); + vec_add_dir(q2, v1, v2, (sd[1] / (sd[1] - sd[2]))); copy_v3_v3(q3, q2); } else { @@ -2842,7 +2862,7 @@ static int ff_visible_quad(const float p[3], const float n[3], const float v0[3] if (sd[2] > 0) { // 0-+ copy_v3_v3(q0, v0); - vec_add_dir(q1, v1, v2, (sd[1]/(sd[1]-sd[2]))); + vec_add_dir(q1, v1, v2, (sd[1] / (sd[1] - sd[2]))); copy_v3_v3(q2, v2); copy_v3_v3(q3, q2); } @@ -2895,7 +2915,7 @@ static vFloat vec_splat_float(float val) static float ff_quad_form_factor(float *p, float *n, float *q0, float *q1, float *q2, float *q3) { vFloat vcos, rlen, vrx, vry, vrz, vsrx, vsry, vsrz, gx, gy, gz, vangle; - vUInt8 rotate = (vUInt8) {4,5,6,7,8,9,10,11,12,13,14,15,0,1,2,3}; + vUInt8 rotate = (vUInt8){4, 5, 6, 7, 8, 9, 10, 11, 12, 13, 14, 15, 0, 1, 2, 3}; vFloatResult vresult; float result; @@ -2905,39 +2925,39 @@ static float ff_quad_form_factor(float *p, float *n, float *q0, float *q1, float vrz = (vFloat) {q0[2], q1[2], q2[2], q3[2]} - vec_splat_float(p[2]); /* normalize r* */ - rlen = vec_rsqrte(vrx*vrx + vry*vry + vrz*vrz + vec_splat_float(1e-16f)); - vrx = vrx*rlen; - vry = vry*rlen; - vrz = vrz*rlen; + rlen = vec_rsqrte(vrx * vrx + vry * vry + vrz * vrz + vec_splat_float(1e-16f)); + vrx = vrx * rlen; + vry = vry * rlen; + vrz = vrz * rlen; /* rotate r* for cross and dot */ - vsrx= vec_perm(vrx, vrx, rotate); - vsry= vec_perm(vry, vry, rotate); - vsrz= vec_perm(vrz, vrz, rotate); + vsrx = vec_perm(vrx, vrx, rotate); + vsry = vec_perm(vry, vry, rotate); + vsrz = vec_perm(vrz, vrz, rotate); /* cross product */ - gx = vsry*vrz - vsrz*vry; - gy = vsrz*vrx - vsrx*vrz; - gz = vsrx*vry - vsry*vrx; + gx = vsry * vrz - vsrz * vry; + gy = vsrz * vrx - vsrx * vrz; + gz = vsrx * vry - vsry * vrx; /* normalize */ - rlen = vec_rsqrte(gx*gx + gy*gy + gz*gz + vec_splat_float(1e-16f)); - gx = gx*rlen; - gy = gy*rlen; - gz = gz*rlen; + rlen = vec_rsqrte(gx * gx + gy * gy + gz * gz + vec_splat_float(1e-16f)); + gx = gx * rlen; + gy = gy * rlen; + gz = gz * rlen; /* angle */ - vcos = vrx*vsrx + vry*vsry + vrz*vsrz; - vcos= vec_max(vec_min(vcos, vec_splat_float(1.0f)), vec_splat_float(-1.0f)); - vangle= vacosf(vcos); + vcos = vrx * vsrx + vry * vsry + vrz * vsrz; + vcos = vec_max(vec_min(vcos, vec_splat_float(1.0f)), vec_splat_float(-1.0f)); + vangle = vacosf(vcos); /* dot */ - vresult.v = (vec_splat_float(n[0])*gx + - vec_splat_float(n[1])*gy + - vec_splat_float(n[2])*gz)*vangle; + vresult.v = (vec_splat_float(n[0]) * gx + + vec_splat_float(n[1]) * gy + + vec_splat_float(n[2]) * gz) * vangle; - result= (vresult.f[0] + vresult.f[1] + vresult.f[2] + vresult.f[3])*(0.5f/(float)M_PI); - result= MAX2(result, 0.0f); + result = (vresult.f[0] + vresult.f[1] + vresult.f[2] + vresult.f[3]) * (0.5f / (float)M_PI); + result = MAX2(result, 0.0f); return result; } @@ -2953,7 +2973,7 @@ static float ff_quad_form_factor(float *p, float *n, float *q0, float *q1, float static __m128 sse_approx_acos(__m128 x) { /* needs a better approximation than taylor expansion of acos, since that - * gives big erros for near 1.0 values, sqrt(2*x)*acos(1-x) should work + * gives big erros for near 1.0 values, sqrt(2 * x) * acos(1 - x) should work * better, see http://www.tom.womack.net/projects/sse-fast-arctrig.html */ return _mm_set_ps1(1.0f); @@ -2976,36 +2996,36 @@ static float ff_quad_form_factor(float *p, float *n, float *q0, float *q1, float rz = qz - _mm_set_ps1(p[2]); /* normalize r */ - rlen = _mm_rsqrt_ps(rx*rx + ry*ry + rz*rz + _mm_set_ps1(1e-16f)); - rx = rx*rlen; - ry = ry*rlen; - rz = rz*rlen; + rlen = _mm_rsqrt_ps(rx * rx + ry * ry + rz * rz + _mm_set_ps1(1e-16f)); + rx = rx * rlen; + ry = ry * rlen; + rz = rz * rlen; /* cross product */ - srx = _mm_shuffle_ps(rx, rx, _MM_SHUFFLE(0,3,2,1)); - sry = _mm_shuffle_ps(ry, ry, _MM_SHUFFLE(0,3,2,1)); - srz = _mm_shuffle_ps(rz, rz, _MM_SHUFFLE(0,3,2,1)); + srx = _mm_shuffle_ps(rx, rx, _MM_SHUFFLE(0, 3, 2, 1)); + sry = _mm_shuffle_ps(ry, ry, _MM_SHUFFLE(0, 3, 2, 1)); + srz = _mm_shuffle_ps(rz, rz, _MM_SHUFFLE(0, 3, 2, 1)); - gx = sry*rz - srz*ry; - gy = srz*rx - srx*rz; - gz = srx*ry - sry*rx; + gx = sry * rz - srz * ry; + gy = srz * rx - srx * rz; + gz = srx * ry - sry * rx; /* normalize g */ - glen = _mm_rsqrt_ps(gx*gx + gy*gy + gz*gz + _mm_set_ps1(1e-16f)); - gx = gx*glen; - gy = gy*glen; - gz = gz*glen; + glen = _mm_rsqrt_ps(gx * gx + gy * gy + gz * gz + _mm_set_ps1(1e-16f)); + gx = gx * glen; + gy = gy * glen; + gz = gz * glen; /* compute angle */ - rcos = rx*srx + ry*sry + rz*srz; - rcos= _mm_max_ps(_mm_min_ps(rcos, _mm_set_ps1(1.0f)), _mm_set_ps1(-1.0f)); + rcos = rx * srx + ry * sry + rz * srz; + rcos = _mm_max_ps(_mm_min_ps(rcos, _mm_set_ps1(1.0f)), _mm_set_ps1(-1.0f)); angle = sse_approx_cos(rcos); - aresult = (_mm_set_ps1(n[0])*gx + _mm_set_ps1(n[1])*gy + _mm_set_ps1(n[2])*gz)*angle; + aresult = (_mm_set_ps1(n[0]) * gx + _mm_set_ps1(n[1]) * gy + _mm_set_ps1(n[2]) * gz) * angle; /* sum together */ - result= (fresult[0] + fresult[1] + fresult[2] + fresult[3])*(0.5f/(float)M_PI); - result= MAX2(result, 0.0f); + result = (fresult[0] + fresult[1] + fresult[2] + fresult[3]) * (0.5f / (float)M_PI); + result = MAX2(result, 0.0f); return result; } @@ -3015,19 +3035,20 @@ static float ff_quad_form_factor(float *p, float *n, float *q0, float *q1, float static void ff_normalize(float n[3]) { float d; - - d= dot_v3v3(n, n); + + d = dot_v3v3(n, n); if (d > 1.0e-35F) { - d= 1.0f/sqrtf(d); + d = 1.0f / sqrtf(d); - n[0] *= d; - n[1] *= d; + n[0] *= d; + n[1] *= d; n[2] *= d; - } + } } -static float ff_quad_form_factor(const float p[3], const float n[3], const float q0[3], const float q1[3], const float q2[3], const float q3[3]) +static float ff_quad_form_factor(const float p[3], const float n[3], + const float q0[3], const float q1[3], const float q2[3], const float q3[3]) { float r0[3], r1[3], r2[3], r3[3], g0[3], g1[3], g2[3], g3[3]; float a1, a2, a3, a4, dot1, dot2, dot3, dot4, result; @@ -3042,23 +3063,27 @@ static float ff_quad_form_factor(const float p[3], const float n[3], const float ff_normalize(r2); ff_normalize(r3); - cross_v3_v3v3(g0, r1, r0); ff_normalize(g0); - cross_v3_v3v3(g1, r2, r1); ff_normalize(g1); - cross_v3_v3v3(g2, r3, r2); ff_normalize(g2); - cross_v3_v3v3(g3, r0, r3); ff_normalize(g3); + cross_v3_v3v3(g0, r1, r0); + ff_normalize(g0); + cross_v3_v3v3(g1, r2, r1); + ff_normalize(g1); + cross_v3_v3v3(g2, r3, r2); + ff_normalize(g2); + cross_v3_v3v3(g3, r0, r3); + ff_normalize(g3); - a1= saacosf(dot_v3v3(r0, r1)); - a2= saacosf(dot_v3v3(r1, r2)); - a3= saacosf(dot_v3v3(r2, r3)); - a4= saacosf(dot_v3v3(r3, r0)); + a1 = saacosf(dot_v3v3(r0, r1)); + a2 = saacosf(dot_v3v3(r1, r2)); + a3 = saacosf(dot_v3v3(r2, r3)); + a4 = saacosf(dot_v3v3(r3, r0)); - dot1= dot_v3v3(n, g0); - dot2= dot_v3v3(n, g1); - dot3= dot_v3v3(n, g2); - dot4= dot_v3v3(n, g3); + dot1 = dot_v3v3(n, g0); + dot2 = dot_v3v3(n, g1); + dot3 = dot_v3v3(n, g2); + dot4 = dot_v3v3(n, g3); - result= (a1*dot1 + a2*dot2 + a3*dot3 + a4*dot4)*0.5f/(float)M_PI; - result= MAX2(result, 0.0f); + result = (a1 * dot1 + a2 * dot2 + a3 * dot3 + a4 * dot4) * 0.5f / (float)M_PI; + result = MAX2(result, 0.0f); return result; } @@ -3067,11 +3092,11 @@ float form_factor_hemi_poly(float p[3], float n[3], float v1[3], float v2[3], fl { /* computes how much hemisphere defined by point and normal is * covered by a quad or triangle, cosine weighted */ - float q0[3], q1[3], q2[3], q3[3], contrib= 0.0f; + float q0[3], q1[3], q2[3], q3[3], contrib = 0.0f; if (ff_visible_quad(p, n, v1, v2, v3, q0, q1, q2, q3)) contrib += ff_quad_form_factor(p, n, q0, q1, q2, q3); - + if (v4 && ff_visible_quad(p, n, v1, v3, v4, q0, q1, q2, q3)) contrib += ff_quad_form_factor(p, n, q0, q1, q2, q3); @@ -3079,8 +3104,8 @@ float form_factor_hemi_poly(float p[3], float n[3], float v1[3], float v2[3], fl } /* evaluate if entire quad is a proper convex quad */ - int is_quad_convex_v3(const float v1[3], const float v2[3], const float v3[3], const float v4[3]) - { +int is_quad_convex_v3(const float v1[3], const float v2[3], const float v3[3], const float v4[3]) +{ float nor[3], nor1[3], nor2[3], vec[4][2]; int axis_a, axis_b; @@ -3102,11 +3127,15 @@ float form_factor_hemi_poly(float p[3], float n[3], float v1[3], float v2[3], fl axis_dominant_v3(&axis_a, &axis_b, nor); - vec[0][0]= v1[axis_a]; vec[0][1]= v1[axis_b]; - vec[1][0]= v2[axis_a]; vec[1][1]= v2[axis_b]; + vec[0][0] = v1[axis_a]; + vec[0][1] = v1[axis_b]; + vec[1][0] = v2[axis_a]; + vec[1][1] = v2[axis_b]; - vec[2][0]= v3[axis_a]; vec[2][1]= v3[axis_b]; - vec[3][0]= v4[axis_a]; vec[3][1]= v4[axis_b]; + vec[2][0] = v3[axis_a]; + vec[2][1] = v3[axis_b]; + vec[3][0] = v4[axis_a]; + vec[3][1] = v4[axis_b]; /* linetests, the 2 diagonals have to instersect to be convex */ return (isect_line_line_v2(vec[0], vec[2], vec[1], vec[3]) > 0) ? TRUE : FALSE; diff --git a/source/blender/blenlib/intern/math_geom_inline.c b/source/blender/blenlib/intern/math_geom_inline.c index 634e68d0ccf..0d4c797cefb 100644 --- a/source/blender/blenlib/intern/math_geom_inline.c +++ b/source/blender/blenlib/intern/math_geom_inline.c @@ -37,19 +37,19 @@ MINLINE void zero_sh(float r[9]) { - memset(r, 0, sizeof(float)*9); + memset(r, 0, sizeof(float) * 9); } MINLINE void copy_sh_sh(float r[9], const float a[9]) { - memcpy(r, a, sizeof(float)*9); + memcpy(r, a, sizeof(float) * 9); } MINLINE void mul_sh_fl(float r[9], const float f) { int i; - for (i=0; i<9; i++) + for (i = 0; i < 9; i++) r[i] *= f; } @@ -57,18 +57,18 @@ MINLINE void add_sh_shsh(float r[9], const float a[9], const float b[9]) { int i; - for (i=0; i<9; i++) - r[i]= a[i] + b[i]; + for (i = 0; i < 9; i++) + r[i] = a[i] + b[i]; } MINLINE float dot_shsh(float a[9], float b[9]) { - float r= 0.0f; + float r = 0.0f; int i; - for (i=0; i<9; i++) - r += a[i]*b[i]; - + for (i = 0; i < 9; i++) + r += a[i] * b[i]; + return r; } @@ -80,16 +80,16 @@ MINLINE float diffuse_shv3(float sh[9], const float v[3]) static const float c4 = 0.886227f, c5 = 0.247708f; float x, y, z, sum; - x= v[0]; - y= v[1]; - z= v[2]; + x = v[0]; + y = v[1]; + z = v[2]; - sum= c1*sh[8]*(x*x - y*y); - sum += c3*sh[6]*z*z; - sum += c4*sh[0]; - sum += -c5*sh[6]; - sum += 2.0f*c1*(sh[4]*x*y + sh[7]*x*z + sh[5]*y*z); - sum += 2.0f*c2*(sh[3]*x + sh[1]*y + sh[2]*z); + sum = c1 * sh[8] * (x * x - y * y); + sum += c3 * sh[6] * z * z; + sum += c4 * sh[0]; + sum += -c5 * sh[6]; + sum += 2.0f * c1 * (sh[4] * x * y + sh[7] * x * z + sh[5] * y * z); + sum += 2.0f * c2 * (sh[3] * x + sh[1] * y + sh[2] * z); return sum; } @@ -100,21 +100,21 @@ MINLINE void vec_fac_to_sh(float r[9], const float v[3], const float f) * "An Efficient Representation for Irradiance Environment Maps" */ float sh[9], x, y, z; - x= v[0]; - y= v[1]; - z= v[2]; - - sh[0]= 0.282095f; - - sh[1]= 0.488603f*y; - sh[2]= 0.488603f*z; - sh[3]= 0.488603f*x; - - sh[4]= 1.092548f*x*y; - sh[5]= 1.092548f*y*z; - sh[6]= 0.315392f*(3.0f*z*z - 1.0f); - sh[7]= 1.092548f*x*z; - sh[8]= 0.546274f*(x*x - y*y); + x = v[0]; + y = v[1]; + z = v[2]; + + sh[0] = 0.282095f; + + sh[1] = 0.488603f * y; + sh[2] = 0.488603f * z; + sh[3] = 0.488603f * x; + + sh[4] = 1.092548f * x * y; + sh[5] = 1.092548f * y * z; + sh[6] = 0.315392f * (3.0f * z * z - 1.0f); + sh[7] = 1.092548f * x * z; + sh[8] = 0.546274f * (x * x - y * y); mul_sh_fl(sh, f); copy_sh_sh(r, sh); diff --git a/source/blender/blenlib/intern/math_matrix.c b/source/blender/blenlib/intern/math_matrix.c index fd49012491e..09b5ab4f62a 100644 --- a/source/blender/blenlib/intern/math_matrix.c +++ b/source/blender/blenlib/intern/math_matrix.c @@ -35,80 +35,80 @@ void zero_m3(float m[3][3]) { - memset(m, 0, 3*3*sizeof(float)); + memset(m, 0, 3 * 3 * sizeof(float)); } void zero_m4(float m[4][4]) { - memset(m, 0, 4*4*sizeof(float)); + memset(m, 0, 4 * 4 * sizeof(float)); } void unit_m3(float m[][3]) { - m[0][0]= m[1][1]= m[2][2]= 1.0; - m[0][1]= m[0][2]= 0.0; - m[1][0]= m[1][2]= 0.0; - m[2][0]= m[2][1]= 0.0; + m[0][0] = m[1][1] = m[2][2] = 1.0; + m[0][1] = m[0][2] = 0.0; + m[1][0] = m[1][2] = 0.0; + m[2][0] = m[2][1] = 0.0; } void unit_m4(float m[][4]) { - m[0][0]= m[1][1]= m[2][2]= m[3][3]= 1.0; - m[0][1]= m[0][2]= m[0][3]= 0.0; - m[1][0]= m[1][2]= m[1][3]= 0.0; - m[2][0]= m[2][1]= m[2][3]= 0.0; - m[3][0]= m[3][1]= m[3][2]= 0.0; + m[0][0] = m[1][1] = m[2][2] = m[3][3] = 1.0; + m[0][1] = m[0][2] = m[0][3] = 0.0; + m[1][0] = m[1][2] = m[1][3] = 0.0; + m[2][0] = m[2][1] = m[2][3] = 0.0; + m[3][0] = m[3][1] = m[3][2] = 0.0; } -void copy_m3_m3(float m1[][3], float m2[][3]) -{ +void copy_m3_m3(float m1[][3], float m2[][3]) +{ /* destination comes first: */ - memcpy(&m1[0], &m2[0], 9*sizeof(float)); + memcpy(&m1[0], &m2[0], 9 * sizeof(float)); } -void copy_m4_m4(float m1[][4], float m2[][4]) +void copy_m4_m4(float m1[][4], float m2[][4]) { - memcpy(m1, m2, 4*4*sizeof(float)); + memcpy(m1, m2, 4 * 4 * sizeof(float)); } void copy_m3_m4(float m1[][3], float m2[][4]) { - m1[0][0]= m2[0][0]; - m1[0][1]= m2[0][1]; - m1[0][2]= m2[0][2]; + m1[0][0] = m2[0][0]; + m1[0][1] = m2[0][1]; + m1[0][2] = m2[0][2]; - m1[1][0]= m2[1][0]; - m1[1][1]= m2[1][1]; - m1[1][2]= m2[1][2]; + m1[1][0] = m2[1][0]; + m1[1][1] = m2[1][1]; + m1[1][2] = m2[1][2]; - m1[2][0]= m2[2][0]; - m1[2][1]= m2[2][1]; - m1[2][2]= m2[2][2]; + m1[2][0] = m2[2][0]; + m1[2][1] = m2[2][1]; + m1[2][2] = m2[2][2]; } -void copy_m4_m3(float m1[][4], float m2[][3]) /* no clear */ +void copy_m4_m3(float m1[][4], float m2[][3]) /* no clear */ { - m1[0][0]= m2[0][0]; - m1[0][1]= m2[0][1]; - m1[0][2]= m2[0][2]; + m1[0][0] = m2[0][0]; + m1[0][1] = m2[0][1]; + m1[0][2] = m2[0][2]; - m1[1][0]= m2[1][0]; - m1[1][1]= m2[1][1]; - m1[1][2]= m2[1][2]; + m1[1][0] = m2[1][0]; + m1[1][1] = m2[1][1]; + m1[1][2] = m2[1][2]; - m1[2][0]= m2[2][0]; - m1[2][1]= m2[2][1]; - m1[2][2]= m2[2][2]; + m1[2][0] = m2[2][0]; + m1[2][1] = m2[2][1]; + m1[2][2] = m2[2][2]; /* Reevan's Bugfix */ - m1[0][3]=0.0F; - m1[1][3]=0.0F; - m1[2][3]=0.0F; + m1[0][3] = 0.0F; + m1[1][3] = 0.0F; + m1[2][3] = 0.0F; - m1[3][0]=0.0F; - m1[3][1]=0.0F; - m1[3][2]=0.0F; - m1[3][3]=1.0F; + m1[3][0] = 0.0F; + m1[3][1] = 0.0F; + m1[3][2] = 0.0F; + m1[3][3] = 1.0F; } @@ -119,7 +119,7 @@ void swap_m3m3(float m1[][3], float m2[][3]) for (i = 0; i < 3; i++) { for (j = 0; j < 3; j++) { - t = m1[i][j]; + t = m1[i][j]; m1[i][j] = m2[i][j]; m2[i][j] = t; } @@ -133,7 +133,7 @@ void swap_m4m4(float m1[][4], float m2[][4]) for (i = 0; i < 4; i++) { for (j = 0; j < 4; j++) { - t = m1[i][j]; + t = m1[i][j]; m1[i][j] = m2[i][j]; m2[i][j] = t; } @@ -151,25 +151,25 @@ void mult_m4_m4m4(float m1[][4], float m3_[][4], float m2_[][4]) copy_m4_m4(m3, m3_); /* matrix product: m1[j][k] = m2[j][i].m3[i][k] */ - m1[0][0] = m2[0][0]*m3[0][0] + m2[0][1]*m3[1][0] + m2[0][2]*m3[2][0] + m2[0][3]*m3[3][0]; - m1[0][1] = m2[0][0]*m3[0][1] + m2[0][1]*m3[1][1] + m2[0][2]*m3[2][1] + m2[0][3]*m3[3][1]; - m1[0][2] = m2[0][0]*m3[0][2] + m2[0][1]*m3[1][2] + m2[0][2]*m3[2][2] + m2[0][3]*m3[3][2]; - m1[0][3] = m2[0][0]*m3[0][3] + m2[0][1]*m3[1][3] + m2[0][2]*m3[2][3] + m2[0][3]*m3[3][3]; + m1[0][0] = m2[0][0] * m3[0][0] + m2[0][1] * m3[1][0] + m2[0][2] * m3[2][0] + m2[0][3] * m3[3][0]; + m1[0][1] = m2[0][0] * m3[0][1] + m2[0][1] * m3[1][1] + m2[0][2] * m3[2][1] + m2[0][3] * m3[3][1]; + m1[0][2] = m2[0][0] * m3[0][2] + m2[0][1] * m3[1][2] + m2[0][2] * m3[2][2] + m2[0][3] * m3[3][2]; + m1[0][3] = m2[0][0] * m3[0][3] + m2[0][1] * m3[1][3] + m2[0][2] * m3[2][3] + m2[0][3] * m3[3][3]; - m1[1][0] = m2[1][0]*m3[0][0] + m2[1][1]*m3[1][0] + m2[1][2]*m3[2][0] + m2[1][3]*m3[3][0]; - m1[1][1] = m2[1][0]*m3[0][1] + m2[1][1]*m3[1][1] + m2[1][2]*m3[2][1] + m2[1][3]*m3[3][1]; - m1[1][2] = m2[1][0]*m3[0][2] + m2[1][1]*m3[1][2] + m2[1][2]*m3[2][2] + m2[1][3]*m3[3][2]; - m1[1][3] = m2[1][0]*m3[0][3] + m2[1][1]*m3[1][3] + m2[1][2]*m3[2][3] + m2[1][3]*m3[3][3]; + m1[1][0] = m2[1][0] * m3[0][0] + m2[1][1] * m3[1][0] + m2[1][2] * m3[2][0] + m2[1][3] * m3[3][0]; + m1[1][1] = m2[1][0] * m3[0][1] + m2[1][1] * m3[1][1] + m2[1][2] * m3[2][1] + m2[1][3] * m3[3][1]; + m1[1][2] = m2[1][0] * m3[0][2] + m2[1][1] * m3[1][2] + m2[1][2] * m3[2][2] + m2[1][3] * m3[3][2]; + m1[1][3] = m2[1][0] * m3[0][3] + m2[1][1] * m3[1][3] + m2[1][2] * m3[2][3] + m2[1][3] * m3[3][3]; - m1[2][0] = m2[2][0]*m3[0][0] + m2[2][1]*m3[1][0] + m2[2][2]*m3[2][0] + m2[2][3]*m3[3][0]; - m1[2][1] = m2[2][0]*m3[0][1] + m2[2][1]*m3[1][1] + m2[2][2]*m3[2][1] + m2[2][3]*m3[3][1]; - m1[2][2] = m2[2][0]*m3[0][2] + m2[2][1]*m3[1][2] + m2[2][2]*m3[2][2] + m2[2][3]*m3[3][2]; - m1[2][3] = m2[2][0]*m3[0][3] + m2[2][1]*m3[1][3] + m2[2][2]*m3[2][3] + m2[2][3]*m3[3][3]; + m1[2][0] = m2[2][0] * m3[0][0] + m2[2][1] * m3[1][0] + m2[2][2] * m3[2][0] + m2[2][3] * m3[3][0]; + m1[2][1] = m2[2][0] * m3[0][1] + m2[2][1] * m3[1][1] + m2[2][2] * m3[2][1] + m2[2][3] * m3[3][1]; + m1[2][2] = m2[2][0] * m3[0][2] + m2[2][1] * m3[1][2] + m2[2][2] * m3[2][2] + m2[2][3] * m3[3][2]; + m1[2][3] = m2[2][0] * m3[0][3] + m2[2][1] * m3[1][3] + m2[2][2] * m3[2][3] + m2[2][3] * m3[3][3]; - m1[3][0] = m2[3][0]*m3[0][0] + m2[3][1]*m3[1][0] + m2[3][2]*m3[2][0] + m2[3][3]*m3[3][0]; - m1[3][1] = m2[3][0]*m3[0][1] + m2[3][1]*m3[1][1] + m2[3][2]*m3[2][1] + m2[3][3]*m3[3][1]; - m1[3][2] = m2[3][0]*m3[0][2] + m2[3][1]*m3[1][2] + m2[3][2]*m3[2][2] + m2[3][3]*m3[3][2]; - m1[3][3] = m2[3][0]*m3[0][3] + m2[3][1]*m3[1][3] + m2[3][2]*m3[2][3] + m2[3][3]*m3[3][3]; + m1[3][0] = m2[3][0] * m3[0][0] + m2[3][1] * m3[1][0] + m2[3][2] * m3[2][0] + m2[3][3] * m3[3][0]; + m1[3][1] = m2[3][0] * m3[0][1] + m2[3][1] * m3[1][1] + m2[3][2] * m3[2][1] + m2[3][3] * m3[3][1]; + m1[3][2] = m2[3][0] * m3[0][2] + m2[3][1] * m3[1][2] + m2[3][2] * m3[2][2] + m2[3][3] * m3[3][2]; + m1[3][3] = m2[3][0] * m3[0][3] + m2[3][1] * m3[1][3] + m2[3][2] * m3[2][3] + m2[3][3] * m3[3][3]; } @@ -181,18 +181,18 @@ void mul_m3_m3m3(float m1[][3], float m3_[][3], float m2_[][3]) copy_m3_m3(m2, m2_); copy_m3_m3(m3, m3_); - /* m1[i][j] = m2[i][k]*m3[k][j], args are flipped! */ - m1[0][0]= m2[0][0]*m3[0][0] + m2[0][1]*m3[1][0] + m2[0][2]*m3[2][0]; - m1[0][1]= m2[0][0]*m3[0][1] + m2[0][1]*m3[1][1] + m2[0][2]*m3[2][1]; - m1[0][2]= m2[0][0]*m3[0][2] + m2[0][1]*m3[1][2] + m2[0][2]*m3[2][2]; + /* m1[i][j] = m2[i][k] * m3[k][j], args are flipped! */ + m1[0][0] = m2[0][0] * m3[0][0] + m2[0][1] * m3[1][0] + m2[0][2] * m3[2][0]; + m1[0][1] = m2[0][0] * m3[0][1] + m2[0][1] * m3[1][1] + m2[0][2] * m3[2][1]; + m1[0][2] = m2[0][0] * m3[0][2] + m2[0][1] * m3[1][2] + m2[0][2] * m3[2][2]; - m1[1][0]= m2[1][0]*m3[0][0] + m2[1][1]*m3[1][0] + m2[1][2]*m3[2][0]; - m1[1][1]= m2[1][0]*m3[0][1] + m2[1][1]*m3[1][1] + m2[1][2]*m3[2][1]; - m1[1][2]= m2[1][0]*m3[0][2] + m2[1][1]*m3[1][2] + m2[1][2]*m3[2][2]; + m1[1][0] = m2[1][0] * m3[0][0] + m2[1][1] * m3[1][0] + m2[1][2] * m3[2][0]; + m1[1][1] = m2[1][0] * m3[0][1] + m2[1][1] * m3[1][1] + m2[1][2] * m3[2][1]; + m1[1][2] = m2[1][0] * m3[0][2] + m2[1][1] * m3[1][2] + m2[1][2] * m3[2][2]; - m1[2][0]= m2[2][0]*m3[0][0] + m2[2][1]*m3[1][0] + m2[2][2]*m3[2][0]; - 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[2][0] = m2[2][0] * m3[0][0] + m2[2][1] * m3[1][0] + m2[2][2] * m3[2][0]; + 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]; } void mul_m4_m4m3(float (*m1)[4], float (*m3_)[4], float (*m2_)[3]) @@ -203,56 +203,56 @@ void mul_m4_m4m3(float (*m1)[4], float (*m3_)[4], float (*m2_)[3]) copy_m3_m3(m2, m2_); copy_m4_m4(m3, m3_); - m1[0][0]= m2[0][0]*m3[0][0] + m2[0][1]*m3[1][0] + m2[0][2]*m3[2][0]; - m1[0][1]= m2[0][0]*m3[0][1] + m2[0][1]*m3[1][1] + m2[0][2]*m3[2][1]; - m1[0][2]= m2[0][0]*m3[0][2] + m2[0][1]*m3[1][2] + m2[0][2]*m3[2][2]; - m1[1][0]= m2[1][0]*m3[0][0] + m2[1][1]*m3[1][0] + m2[1][2]*m3[2][0]; - m1[1][1]= m2[1][0]*m3[0][1] + m2[1][1]*m3[1][1] + m2[1][2]*m3[2][1]; - m1[1][2]= m2[1][0]*m3[0][2] + m2[1][1]*m3[1][2] + m2[1][2]*m3[2][2]; - m1[2][0]= m2[2][0]*m3[0][0] + m2[2][1]*m3[1][0] + m2[2][2]*m3[2][0]; - 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[0][0] = m2[0][0] * m3[0][0] + m2[0][1] * m3[1][0] + m2[0][2] * m3[2][0]; + m1[0][1] = m2[0][0] * m3[0][1] + m2[0][1] * m3[1][1] + m2[0][2] * m3[2][1]; + m1[0][2] = m2[0][0] * m3[0][2] + m2[0][1] * m3[1][2] + m2[0][2] * m3[2][2]; + m1[1][0] = m2[1][0] * m3[0][0] + m2[1][1] * m3[1][0] + m2[1][2] * m3[2][0]; + m1[1][1] = m2[1][0] * m3[0][1] + m2[1][1] * m3[1][1] + m2[1][2] * m3[2][1]; + m1[1][2] = m2[1][0] * m3[0][2] + m2[1][1] * m3[1][2] + m2[1][2] * m3[2][2]; + m1[2][0] = m2[2][0] * m3[0][0] + m2[2][1] * m3[1][0] + m2[2][2] * m3[2][0]; + 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*/ +/* m1 = m2 * m3, ignore the elements on the 4th row/column of m3 */ void mult_m3_m3m4(float m1[][3], float m3[][4], float m2[][3]) { /* m1[i][j] = m2[i][k] * m3[k][j] */ - m1[0][0] = m2[0][0] * m3[0][0] + m2[0][1] * m3[1][0] +m2[0][2] * m3[2][0]; - m1[0][1] = m2[0][0] * m3[0][1] + m2[0][1] * m3[1][1] +m2[0][2] * m3[2][1]; - m1[0][2] = m2[0][0] * m3[0][2] + m2[0][1] * m3[1][2] +m2[0][2] * m3[2][2]; + m1[0][0] = m2[0][0] * m3[0][0] + m2[0][1] * m3[1][0] + m2[0][2] * m3[2][0]; + m1[0][1] = m2[0][0] * m3[0][1] + m2[0][1] * m3[1][1] + m2[0][2] * m3[2][1]; + m1[0][2] = m2[0][0] * m3[0][2] + m2[0][1] * m3[1][2] + m2[0][2] * m3[2][2]; - m1[1][0] = m2[1][0] * m3[0][0] + m2[1][1] * m3[1][0] +m2[1][2] * m3[2][0]; - m1[1][1] = m2[1][0] * m3[0][1] + m2[1][1] * m3[1][1] +m2[1][2] * m3[2][1]; - m1[1][2] = m2[1][0] * m3[0][2] + m2[1][1] * m3[1][2] +m2[1][2] * m3[2][2]; + m1[1][0] = m2[1][0] * m3[0][0] + m2[1][1] * m3[1][0] + m2[1][2] * m3[2][0]; + m1[1][1] = m2[1][0] * m3[0][1] + m2[1][1] * m3[1][1] + m2[1][2] * m3[2][1]; + m1[1][2] = m2[1][0] * m3[0][2] + m2[1][1] * m3[1][2] + m2[1][2] * m3[2][2]; - m1[2][0] = m2[2][0] * m3[0][0] + m2[2][1] * m3[1][0] +m2[2][2] * m3[2][0]; - 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[2][0] = m2[2][0] * m3[0][0] + m2[2][1] * m3[1][0] + m2[2][2] * m3[2][0]; + 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]; } 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]; - m1[0][1]= m2[0][0]*m3[0][1] + m2[0][1]*m3[1][1] + m2[0][2]*m3[2][1]; - m1[0][2]= m2[0][0]*m3[0][2] + m2[0][1]*m3[1][2] + m2[0][2]*m3[2][2]; - m1[1][0]= m2[1][0]*m3[0][0] + m2[1][1]*m3[1][0] + m2[1][2]*m3[2][0]; - m1[1][1]= m2[1][0]*m3[0][1] + m2[1][1]*m3[1][1] + m2[1][2]*m3[2][1]; - m1[1][2]= m2[1][0]*m3[0][2] + m2[1][1]*m3[1][2] + m2[1][2]*m3[2][2]; - m1[2][0]= m2[2][0]*m3[0][0] + m2[2][1]*m3[1][0] + m2[2][2]*m3[2][0]; - 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[0][0] = m2[0][0] * m3[0][0] + m2[0][1] * m3[1][0] + m2[0][2] * m3[2][0]; + m1[0][1] = m2[0][0] * m3[0][1] + m2[0][1] * m3[1][1] + m2[0][2] * m3[2][1]; + m1[0][2] = m2[0][0] * m3[0][2] + m2[0][1] * m3[1][2] + m2[0][2] * m3[2][2]; + m1[1][0] = m2[1][0] * m3[0][0] + m2[1][1] * m3[1][0] + m2[1][2] * m3[2][0]; + m1[1][1] = m2[1][0] * m3[0][1] + m2[1][1] * m3[1][1] + m2[1][2] * m3[2][1]; + m1[1][2] = m2[1][0] * m3[0][2] + m2[1][1] * m3[1][2] + m2[1][2] * m3[2][2]; + m1[2][0] = m2[2][0] * m3[0][0] + m2[2][1] * m3[1][0] + m2[2][2] * m3[2][0]; + 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]; } void mul_serie_m3(float answ[][3], - float m1[][3], float m2[][3], float m3[][3], - float m4[][3], float m5[][3], float m6[][3], - float m7[][3], float m8[][3]) +float m1[][3], float m2[][3], float m3[][3], +float m4[][3], float m5[][3], float m6[][3], +float m7[][3], float m8[][3]) { float temp[3][3]; - - if (m1==NULL || m2==NULL) return; - + + if (m1 == NULL || m2 == NULL) return; + mul_m3_m3m3(answ, m2, m1); if (m3) { mul_m3_m3m3(temp, m3, answ); @@ -278,14 +278,14 @@ void mul_serie_m3(float answ[][3], } void mul_serie_m4(float answ[][4], float m1[][4], - float m2[][4], float m3[][4], float m4[][4], - float m5[][4], float m6[][4], float m7[][4], - float m8[][4]) +float m2[][4], float m3[][4], float m4[][4], +float m5[][4], float m6[][4], float m7[][4], +float m8[][4]) { float temp[4][4]; - - if (m1==NULL || m2==NULL) return; - + + if (m1 == NULL || m2 == NULL) return; + mult_m4_m4m4(answ, m1, m2); if (m3) { mult_m4_m4m4(temp, answ, m3); @@ -312,41 +312,41 @@ void mul_serie_m4(float answ[][4], float m1[][4], void mul_m4_v3(float mat[][4], float vec[3]) { - float x,y; + float x, y; - x=vec[0]; - y=vec[1]; - vec[0]=x*mat[0][0] + y*mat[1][0] + mat[2][0]*vec[2] + mat[3][0]; - vec[1]=x*mat[0][1] + y*mat[1][1] + mat[2][1]*vec[2] + mat[3][1]; - vec[2]=x*mat[0][2] + y*mat[1][2] + mat[2][2]*vec[2] + mat[3][2]; + x = vec[0]; + y = vec[1]; + vec[0] = x * mat[0][0] + y * mat[1][0] + mat[2][0] * vec[2] + mat[3][0]; + vec[1] = x * mat[0][1] + y * mat[1][1] + mat[2][1] * vec[2] + mat[3][1]; + vec[2] = x * mat[0][2] + y * mat[1][2] + mat[2][2] * vec[2] + mat[3][2]; } void mul_v3_m4v3(float in[3], float mat[][4], const float vec[3]) { - float x,y; + float x, y; - x=vec[0]; - y=vec[1]; - in[0]= x*mat[0][0] + y*mat[1][0] + mat[2][0]*vec[2] + mat[3][0]; - in[1]= x*mat[0][1] + y*mat[1][1] + mat[2][1]*vec[2] + mat[3][1]; - in[2]= x*mat[0][2] + y*mat[1][2] + mat[2][2]*vec[2] + mat[3][2]; + x = vec[0]; + y = vec[1]; + in[0] = x * mat[0][0] + y * mat[1][0] + mat[2][0] * vec[2] + mat[3][0]; + in[1] = x * mat[0][1] + y * mat[1][1] + mat[2][1] * vec[2] + mat[3][1]; + in[2] = x * mat[0][2] + y * mat[1][2] + mat[2][2] * vec[2] + mat[3][2]; } /* same as mul_m4_v3() but doesnt apply translation component */ void mul_mat3_m4_v3(float mat[][4], float vec[3]) { - float x,y; + float x, y; - x= vec[0]; - y= vec[1]; - vec[0]= x*mat[0][0] + y*mat[1][0] + mat[2][0]*vec[2]; - vec[1]= x*mat[0][1] + y*mat[1][1] + mat[2][1]*vec[2]; - vec[2]= x*mat[0][2] + y*mat[1][2] + mat[2][2]*vec[2]; + x = vec[0]; + y = vec[1]; + vec[0] = x * mat[0][0] + y * mat[1][0] + mat[2][0] * vec[2]; + vec[1] = x * mat[0][1] + y * mat[1][1] + mat[2][1] * vec[2]; + vec[2] = x * mat[0][2] + y * mat[1][2] + mat[2][2] * vec[2]; } void mul_project_m4_v3(float mat[][4], float vec[3]) { - const float w= vec[0]*mat[0][3] + vec[1]*mat[1][3] + vec[2]*mat[2][3] + mat[3][3]; + const float w = vec[0] * mat[0][3] + vec[1] * mat[1][3] + vec[2] * mat[2][3] + mat[3][3]; mul_m4_v3(mat, vec); vec[0] /= w; @@ -358,14 +358,14 @@ void mul_v4_m4v4(float r[4], float mat[4][4], float v[4]) { float x, y, z; - x= v[0]; - y= v[1]; - z= v[2]; + x = v[0]; + y = v[1]; + z = v[2]; - r[0]= x*mat[0][0] + y*mat[1][0] + z*mat[2][0] + mat[3][0]*v[3]; - r[1]= x*mat[0][1] + y*mat[1][1] + z*mat[2][1] + mat[3][1]*v[3]; - r[2]= x*mat[0][2] + y*mat[1][2] + z*mat[2][2] + mat[3][2]*v[3]; - r[3]= x*mat[0][3] + y*mat[1][3] + z*mat[2][3] + mat[3][3]*v[3]; + r[0] = x * mat[0][0] + y * mat[1][0] + z * mat[2][0] + mat[3][0] * v[3]; + r[1] = x * mat[0][1] + y * mat[1][1] + z * mat[2][1] + mat[3][1] * v[3]; + r[2] = x * mat[0][2] + y * mat[1][2] + z * mat[2][2] + mat[3][2] * v[3]; + r[3] = x * mat[0][3] + y * mat[1][3] + z * mat[2][3] + mat[3][3] * v[3]; } void mul_m4_v4(float mat[4][4], float r[4]) @@ -377,14 +377,14 @@ void mul_v4d_m4v4d(double r[4], float mat[4][4], double v[4]) { double x, y, z; - x= v[0]; - y= v[1]; - z= v[2]; + x = v[0]; + y = v[1]; + z = v[2]; - r[0]= x*(double)mat[0][0] + y*(double)mat[1][0] + z*(double)mat[2][0] + (double)mat[3][0]*v[3]; - r[1]= x*(double)mat[0][1] + y*(double)mat[1][1] + z*(double)mat[2][1] + (double)mat[3][1]*v[3]; - r[2]= x*(double)mat[0][2] + y*(double)mat[1][2] + z*(double)mat[2][2] + (double)mat[3][2]*v[3]; - r[3]= x*(double)mat[0][3] + y*(double)mat[1][3] + z*(double)mat[2][3] + (double)mat[3][3]*v[3]; + r[0] = x * (double)mat[0][0] + y * (double)mat[1][0] + z * (double)mat[2][0] + (double)mat[3][0] * v[3]; + r[1] = x * (double)mat[0][1] + y * (double)mat[1][1] + z * (double)mat[2][1] + (double)mat[3][1] * v[3]; + r[2] = x * (double)mat[0][2] + y * (double)mat[1][2] + z * (double)mat[2][2] + (double)mat[3][2] * v[3]; + r[3] = x * (double)mat[0][3] + y * (double)mat[1][3] + z * (double)mat[2][3] + (double)mat[3][3] * v[3]; } void mul_m4_v4d(float mat[4][4], double r[4]) @@ -394,9 +394,9 @@ void mul_m4_v4d(float mat[4][4], double r[4]) void mul_v3_m3v3(float r[3], float M[3][3], float a[3]) { - r[0]= M[0][0]*a[0] + M[1][0]*a[1] + M[2][0]*a[2]; - r[1]= M[0][1]*a[0] + M[1][1]*a[1] + M[2][1]*a[2]; - r[2]= M[0][2]*a[0] + M[1][2]*a[1] + M[2][2]*a[2]; + r[0] = M[0][0] * a[0] + M[1][0] * a[1] + M[2][0] * a[2]; + r[1] = M[0][1] * a[0] + M[1][1] * a[1] + M[2][1] * a[2]; + r[2] = M[0][2] * a[0] + M[1][2] * a[1] + M[2][2] * a[2]; } void mul_m3_v3(float M[3][3], float r[3]) @@ -409,21 +409,21 @@ void mul_m3_v3(float M[3][3], float r[3]) void mul_transposed_m3_v3(float mat[][3], float vec[3]) { - float x,y; + float x, y; - x=vec[0]; - y=vec[1]; - vec[0]= x*mat[0][0] + y*mat[0][1] + mat[0][2]*vec[2]; - vec[1]= x*mat[1][0] + y*mat[1][1] + mat[1][2]*vec[2]; - vec[2]= x*mat[2][0] + y*mat[2][1] + mat[2][2]*vec[2]; + x = vec[0]; + y = vec[1]; + vec[0] = x * mat[0][0] + y * mat[0][1] + mat[0][2] * vec[2]; + vec[1] = x * mat[1][0] + y * mat[1][1] + mat[1][2] * vec[2]; + vec[2] = x * mat[2][0] + y * mat[2][1] + mat[2][2] * vec[2]; } void mul_m3_fl(float m[3][3], float f) { int i, j; - for (i=0;i<3;i++) - for (j=0;j<3;j++) + for (i = 0; i < 3; i++) + for (j = 0; j < 3; j++) m[i][j] *= f; } @@ -431,8 +431,8 @@ void mul_m4_fl(float m[4][4], float f) { int i, j; - for (i=0;i<4;i++) - for (j=0;j<4;j++) + for (i = 0; i < 4; i++) + for (j = 0; j < 4; j++) m[i][j] *= f; } @@ -440,56 +440,56 @@ void mul_mat3_m4_fl(float m[4][4], float f) { int i, j; - for (i=0; i<3; i++) - for (j=0; j<3; j++) + 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[3]) { - double x,y; + double x, y; - x=vec[0]; - y=vec[1]; - vec[0]= x*(double)mat[0][0] + y*(double)mat[1][0] + (double)mat[2][0]*vec[2]; - vec[1]= x*(double)mat[0][1] + y*(double)mat[1][1] + (double)mat[2][1]*vec[2]; - vec[2]= x*(double)mat[0][2] + y*(double)mat[1][2] + (double)mat[2][2]*vec[2]; + x = vec[0]; + y = vec[1]; + vec[0] = x * (double)mat[0][0] + y * (double)mat[1][0] + (double)mat[2][0] * vec[2]; + vec[1] = x * (double)mat[0][1] + y * (double)mat[1][1] + (double)mat[2][1] * vec[2]; + vec[2] = x * (double)mat[0][2] + y * (double)mat[1][2] + (double)mat[2][2] * vec[2]; } void add_m3_m3m3(float m1[][3], float m2[][3], float m3[][3]) { int i, j; - for (i=0;i<3;i++) - for (j=0;j<3;j++) - m1[i][j]= m2[i][j] + m3[i][j]; + for (i = 0; i < 3; i++) + for (j = 0; j < 3; j++) + m1[i][j] = m2[i][j] + m3[i][j]; } void add_m4_m4m4(float m1[][4], float m2[][4], float m3[][4]) { int i, j; - for (i=0;i<4;i++) - for (j=0;j<4;j++) - m1[i][j]= m2[i][j] + m3[i][j]; + for (i = 0; i < 4; i++) + for (j = 0; j < 4; j++) + m1[i][j] = m2[i][j] + m3[i][j]; } void sub_m3_m3m3(float m1[][3], float m2[][3], float m3[][3]) { int i, j; - for (i=0;i<3;i++) - for (j=0;j<3;j++) - m1[i][j]= m2[i][j] - m3[i][j]; + for (i = 0; i < 3; i++) + for (j = 0; j < 3; j++) + m1[i][j] = m2[i][j] - m3[i][j]; } void sub_m4_m4m4(float m1[][4], float m2[][4], float m3[][4]) { int i, j; - for (i=0;i<4;i++) - for (j=0;j<4;j++) - m1[i][j]= m2[i][j] - m3[i][j]; + for (i = 0; i < 4; i++) + for (j = 0; j < 4; j++) + m1[i][j] = m2[i][j] - m3[i][j]; } int invert_m3(float m[3][3]) @@ -497,7 +497,7 @@ int invert_m3(float m[3][3]) float tmp[3][3]; int success; - success= invert_m3_m3(tmp, m); + success = invert_m3_m3(tmp, m); copy_m3_m3(m, tmp); return success; @@ -509,20 +509,20 @@ int invert_m3_m3(float m1[3][3], float m2[3][3]) int a, b, success; /* calc adjoint */ - adjoint_m3_m3(m1,m2); + adjoint_m3_m3(m1, m2); /* then determinant old matrix! */ - det= m2[0][0]* (m2[1][1]*m2[2][2] - m2[1][2]*m2[2][1]) - -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]); + det = (m2[0][0] * (m2[1][1] * m2[2][2] - m2[1][2] * m2[2][1]) - + 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); + success = (det != 0); - if (det==0) det=1; - det= 1/det; - for (a=0;a<3;a++) { - for (b=0;b<3;b++) { - m1[a][b]*=det; + if (det == 0) det = 1; + det = 1 / det; + for (a = 0; a < 3; a++) { + for (b = 0; b < 3; b++) { + m1[a][b] *= det; } } @@ -534,15 +534,15 @@ int invert_m4(float m[4][4]) float tmp[4][4]; int success; - success= invert_m4_m4(tmp, m); + success = invert_m4_m4(tmp, m); copy_m4_m4(m, tmp); return success; } /* - * invertmat - - * computes the inverse of mat and puts it in inverse. Returns + * invertmat - + * computes the inverse of mat and puts it in inverse. Returns * TRUE on success (i.e. can always find a pivot) and FALSE on failure. * Uses Gaussian Elimination with partial (maximal column) pivoting. * @@ -558,15 +558,15 @@ int invert_m4_m4(float inverse[4][4], float mat[4][4]) int maxj; /* Set inverse to identity */ - for (i=0; i<4; i++) - for (j=0; j<4; j++) + for (i = 0; i < 4; i++) + for (j = 0; j < 4; j++) inverse[i][j] = 0; - for (i=0; i<4; i++) + for (i = 0; i < 4; i++) inverse[i][i] = 1; /* Copy original matrix so we don't mess it up */ for (i = 0; i < 4; i++) - for (j = 0; j <4; j++) + for (j = 0; j < 4; j++) tempmat[i][j] = mat[i][j]; for (i = 0; i < 4; i++) { @@ -589,17 +589,17 @@ int invert_m4_m4(float inverse[4][4], float mat[4][4]) temp = tempmat[i][i]; if (temp == 0) - return 0; /* No non-zero pivot */ + return 0; /* No non-zero pivot */ for (k = 0; k < 4; k++) { - tempmat[i][k] = (float)(tempmat[i][k]/temp); - inverse[i][k] = (float)(inverse[i][k]/temp); + tempmat[i][k] = (float)(tempmat[i][k] / temp); + inverse[i][k] = (float)(inverse[i][k] / temp); } for (j = 0; j < 4; j++) { if (j != i) { temp = tempmat[j][i]; for (k = 0; k < 4; k++) { - tempmat[j][k] -= (float)(tempmat[i][k]*temp); - inverse[j][k] -= (float)(inverse[i][k]*temp); + tempmat[j][k] -= (float)(tempmat[i][k] * temp); + inverse[j][k] -= (float)(inverse[i][k] * temp); } } } @@ -655,8 +655,7 @@ void orthogonalize_m3(float mat[][3], int axis) float size[3]; mat3_to_size(size, mat); normalize_v3(mat[axis]); - switch(axis) - { + switch (axis) { case 0: if (dot_v3v3(mat[0], mat[1]) < 1) { cross_v3_v3v3(mat[2], mat[0], mat[1]); @@ -671,9 +670,9 @@ void orthogonalize_m3(float mat[][3], int axis) else { float vec[3]; - vec[0]= mat[0][1]; - vec[1]= mat[0][2]; - vec[2]= mat[0][0]; + vec[0] = mat[0][1]; + vec[1] = mat[0][2]; + vec[2] = mat[0][0]; cross_v3_v3v3(mat[2], mat[0], vec); normalize_v3(mat[2]); @@ -693,9 +692,9 @@ void orthogonalize_m3(float mat[][3], int axis) else { float vec[3]; - vec[0]= mat[1][1]; - vec[1]= mat[1][2]; - vec[2]= mat[1][0]; + vec[0] = mat[1][1]; + vec[1] = mat[1][2]; + vec[2] = mat[1][0]; cross_v3_v3v3(mat[0], mat[1], vec); normalize_v3(mat[0]); @@ -715,9 +714,9 @@ void orthogonalize_m3(float mat[][3], int axis) else { float vec[3]; - vec[0]= mat[2][1]; - vec[1]= mat[2][2]; - vec[2]= mat[2][0]; + vec[0] = mat[2][1]; + vec[1] = mat[2][2]; + vec[2] = mat[2][0]; cross_v3_v3v3(mat[0], vec, mat[2]); normalize_v3(mat[0]); @@ -734,8 +733,7 @@ void orthogonalize_m4(float mat[][4], int axis) float size[3]; mat4_to_size(size, mat); normalize_v3(mat[axis]); - switch(axis) - { + switch (axis) { case 0: if (dot_v3v3(mat[0], mat[1]) < 1) { cross_v3_v3v3(mat[2], mat[0], mat[1]); @@ -750,9 +748,9 @@ void orthogonalize_m4(float mat[][4], int axis) else { float vec[3]; - vec[0]= mat[0][1]; - vec[1]= mat[0][2]; - vec[2]= mat[0][0]; + vec[0] = mat[0][1]; + vec[1] = mat[0][2]; + vec[2] = mat[0][0]; cross_v3_v3v3(mat[2], mat[0], vec); normalize_v3(mat[2]); @@ -773,9 +771,9 @@ void orthogonalize_m4(float mat[][4], int axis) else { float vec[3]; - vec[0]= mat[1][1]; - vec[1]= mat[1][2]; - vec[2]= mat[1][0]; + vec[0] = mat[1][1]; + vec[1] = mat[1][2]; + vec[2] = mat[1][0]; cross_v3_v3v3(mat[0], mat[1], vec); normalize_v3(mat[0]); @@ -795,9 +793,9 @@ void orthogonalize_m4(float mat[][4], int axis) else { float vec[3]; - vec[0]= mat[2][1]; - vec[1]= mat[2][2]; - vec[2]= mat[2][0]; + vec[0] = mat[2][1]; + vec[1] = mat[2][2]; + vec[2] = mat[2][0]; cross_v3_v3v3(mat[0], vec, mat[2]); normalize_v3(mat[0]); @@ -844,121 +842,120 @@ int is_orthogonal_m4(float m[][4]) } void normalize_m3(float mat[][3]) -{ +{ normalize_v3(mat[0]); normalize_v3(mat[1]); normalize_v3(mat[2]); } void normalize_m3_m3(float rmat[][3], float mat[][3]) -{ +{ normalize_v3_v3(rmat[0], mat[0]); normalize_v3_v3(rmat[1], mat[1]); normalize_v3_v3(rmat[2], mat[2]); } - void normalize_m4(float mat[][4]) { float len; - - len= normalize_v3(mat[0]); - if (len!=0.0f) mat[0][3]/= len; - len= normalize_v3(mat[1]); - if (len!=0.0f) mat[1][3]/= len; - len= normalize_v3(mat[2]); - if (len!=0.0f) mat[2][3]/= len; + + len = normalize_v3(mat[0]); + if (len != 0.0f) mat[0][3] /= len; + len = normalize_v3(mat[1]); + if (len != 0.0f) mat[1][3] /= len; + len = normalize_v3(mat[2]); + if (len != 0.0f) mat[2][3] /= len; } void normalize_m4_m4(float rmat[][4], float mat[][4]) { float len; - - len= normalize_v3_v3(rmat[0], mat[0]); - if (len!=0.0f) rmat[0][3]= mat[0][3] / len; - len= normalize_v3_v3(rmat[1], mat[1]); - if (len!=0.0f) rmat[1][3]= mat[1][3] / len; - len= normalize_v3_v3(rmat[2], mat[2]); - if (len!=0.0f) rmat[2][3]= mat[2][3] / len; + + len = normalize_v3_v3(rmat[0], mat[0]); + if (len != 0.0f) rmat[0][3] = mat[0][3] / len; + len = normalize_v3_v3(rmat[1], mat[1]); + if (len != 0.0f) rmat[1][3] = mat[1][3] / len; + len = normalize_v3_v3(rmat[2], mat[2]); + if (len != 0.0f) rmat[2][3] = mat[2][3] / len; } void adjoint_m3_m3(float m1[][3], float m[][3]) { - m1[0][0]=m[1][1]*m[2][2]-m[1][2]*m[2][1]; - m1[0][1]= -m[0][1]*m[2][2]+m[0][2]*m[2][1]; - m1[0][2]=m[0][1]*m[1][2]-m[0][2]*m[1][1]; + m1[0][0] = m[1][1] * m[2][2] - m[1][2] * m[2][1]; + m1[0][1] = -m[0][1] * m[2][2] + m[0][2] * m[2][1]; + m1[0][2] = m[0][1] * m[1][2] - m[0][2] * m[1][1]; - m1[1][0]= -m[1][0]*m[2][2]+m[1][2]*m[2][0]; - m1[1][1]=m[0][0]*m[2][2]-m[0][2]*m[2][0]; - m1[1][2]= -m[0][0]*m[1][2]+m[0][2]*m[1][0]; + m1[1][0] = -m[1][0] * m[2][2] + m[1][2] * m[2][0]; + m1[1][1] = m[0][0] * m[2][2] - m[0][2] * m[2][0]; + m1[1][2] = -m[0][0] * m[1][2] + m[0][2] * m[1][0]; - m1[2][0]=m[1][0]*m[2][1]-m[1][1]*m[2][0]; - m1[2][1]= -m[0][0]*m[2][1]+m[0][1]*m[2][0]; - m1[2][2]=m[0][0]*m[1][1]-m[0][1]*m[1][0]; + m1[2][0] = m[1][0] * m[2][1] - m[1][1] * m[2][0]; + m1[2][1] = -m[0][0] * m[2][1] + m[0][1] * m[2][0]; + m1[2][2] = m[0][0] * m[1][1] - m[0][1] * m[1][0]; } -void adjoint_m4_m4(float out[][4], float in[][4]) /* out = ADJ(in) */ +void adjoint_m4_m4(float out[][4], float in[][4]) /* out = ADJ(in) */ { float a1, a2, a3, a4, b1, b2, b3, b4; float c1, c2, c3, c4, d1, d2, d3, d4; - a1= in[0][0]; - b1= in[0][1]; - c1= in[0][2]; - d1= in[0][3]; + a1 = in[0][0]; + b1 = in[0][1]; + c1 = in[0][2]; + d1 = in[0][3]; - a2= in[1][0]; - b2= in[1][1]; - c2= in[1][2]; - d2= in[1][3]; + a2 = in[1][0]; + b2 = in[1][1]; + c2 = in[1][2]; + d2 = in[1][3]; - a3= in[2][0]; - b3= in[2][1]; - c3= in[2][2]; - d3= in[2][3]; + a3 = in[2][0]; + b3 = in[2][1]; + c3 = in[2][2]; + d3 = in[2][3]; - a4= in[3][0]; - b4= in[3][1]; - c4= in[3][2]; - d4= in[3][3]; + a4 = in[3][0]; + b4 = in[3][1]; + c4 = in[3][2]; + d4 = in[3][3]; - out[0][0] = determinant_m3(b2, b3, b4, c2, c3, c4, d2, d3, d4); - out[1][0] = - determinant_m3(a2, a3, a4, c2, c3, c4, d2, d3, d4); - out[2][0] = determinant_m3(a2, a3, a4, b2, b3, b4, d2, d3, d4); - out[3][0] = - determinant_m3(a2, a3, a4, b2, b3, b4, c2, c3, c4); + out[0][0] = determinant_m3(b2, b3, b4, c2, c3, c4, d2, d3, d4); + out[1][0] = -determinant_m3(a2, a3, a4, c2, c3, c4, d2, d3, d4); + out[2][0] = determinant_m3(a2, a3, a4, b2, b3, b4, d2, d3, d4); + out[3][0] = -determinant_m3(a2, a3, a4, b2, b3, b4, c2, c3, c4); - out[0][1] = - determinant_m3(b1, b3, b4, c1, c3, c4, d1, d3, d4); - out[1][1] = determinant_m3(a1, a3, a4, c1, c3, c4, d1, d3, d4); - out[2][1] = - determinant_m3(a1, a3, a4, b1, b3, b4, d1, d3, d4); - out[3][1] = determinant_m3(a1, a3, a4, b1, b3, b4, c1, c3, c4); + out[0][1] = -determinant_m3(b1, b3, b4, c1, c3, c4, d1, d3, d4); + out[1][1] = determinant_m3(a1, a3, a4, c1, c3, c4, d1, d3, d4); + out[2][1] = -determinant_m3(a1, a3, a4, b1, b3, b4, d1, d3, d4); + out[3][1] = determinant_m3(a1, a3, a4, b1, b3, b4, c1, c3, c4); - out[0][2] = determinant_m3(b1, b2, b4, c1, c2, c4, d1, d2, d4); - out[1][2] = - determinant_m3(a1, a2, a4, c1, c2, c4, d1, d2, d4); - out[2][2] = determinant_m3(a1, a2, a4, b1, b2, b4, d1, d2, d4); - out[3][2] = - determinant_m3(a1, a2, a4, b1, b2, b4, c1, c2, c4); + out[0][2] = determinant_m3(b1, b2, b4, c1, c2, c4, d1, d2, d4); + out[1][2] = -determinant_m3(a1, a2, a4, c1, c2, c4, d1, d2, d4); + out[2][2] = determinant_m3(a1, a2, a4, b1, b2, b4, d1, d2, d4); + out[3][2] = -determinant_m3(a1, a2, a4, b1, b2, b4, c1, c2, c4); - out[0][3] = - determinant_m3(b1, b2, b3, c1, c2, c3, d1, d2, d3); - out[1][3] = determinant_m3(a1, a2, a3, c1, c2, c3, d1, d2, d3); - out[2][3] = - determinant_m3(a1, a2, a3, b1, b2, b3, d1, d2, d3); - out[3][3] = determinant_m3(a1, a2, a3, b1, b2, b3, c1, c2, c3); + out[0][3] = -determinant_m3(b1, b2, b3, c1, c2, c3, d1, d2, d3); + out[1][3] = determinant_m3(a1, a2, a3, c1, c2, c3, d1, d2, d3); + out[2][3] = -determinant_m3(a1, a2, a3, b1, b2, b3, d1, d2, d3); + out[3][3] = determinant_m3(a1, a2, a3, b1, b2, b3, c1, c2, c3); } -float determinant_m2(float a,float b,float c,float d) +float determinant_m2(float a, float b, float c, float d) { - return a*d - b*c; + return a * d - b * c; } float determinant_m3(float a1, float a2, float a3, - float b1, float b2, float b3, - float c1, float c2, float c3) + float b1, float b2, float b3, + float c1, float c2, float c3) { float ans; - ans = a1 * determinant_m2(b2, b3, c2, c3) - - b1 * determinant_m2(a2, a3, c2, c3) - + c1 * determinant_m2(a2, a3, b2, b3); + ans = (a1 * determinant_m2(b2, b3, c2, c3) - + b1 * determinant_m2(a2, a3, c2, c3) + + c1 * determinant_m2(a2, a3, b2, b3)); return ans; } @@ -966,32 +963,32 @@ float determinant_m3(float a1, float a2, float a3, float determinant_m4(float m[][4]) { float ans; - float a1,a2,a3,a4,b1,b2,b3,b4,c1,c2,c3,c4,d1,d2,d3,d4; + float a1, a2, a3, a4, b1, b2, b3, b4, c1, c2, c3, c4, d1, d2, d3, d4; - a1= m[0][0]; - b1= m[0][1]; - c1= m[0][2]; - d1= m[0][3]; + a1 = m[0][0]; + b1 = m[0][1]; + c1 = m[0][2]; + d1 = m[0][3]; - a2= m[1][0]; - b2= m[1][1]; - c2= m[1][2]; - d2= m[1][3]; + a2 = m[1][0]; + b2 = m[1][1]; + c2 = m[1][2]; + d2 = m[1][3]; - a3= m[2][0]; - b3= m[2][1]; - c3= m[2][2]; - d3= m[2][3]; + a3 = m[2][0]; + b3 = m[2][1]; + c3 = m[2][2]; + d3 = m[2][3]; - a4= m[3][0]; - b4= m[3][1]; - c4= m[3][2]; - d4= m[3][3]; + a4 = m[3][0]; + b4 = m[3][1]; + c4 = m[3][2]; + d4 = m[3][3]; - ans = a1 * determinant_m3(b2, b3, b4, c2, c3, c4, d2, d3, d4) - - b1 * determinant_m3(a2, a3, a4, c2, c3, c4, d2, d3, d4) - + c1 * determinant_m3(a2, a3, a4, b2, b3, b4, d2, d3, d4) - - d1 * determinant_m3(a2, a3, a4, b2, b3, b4, c2, c3, c4); + ans = (a1 * determinant_m3(b2, b3, b4, c2, c3, c4, d2, d3, d4) - + b1 * determinant_m3(a2, a3, a4, c2, c3, c4, d2, d3, d4) + + c1 * determinant_m3(a2, a3, a4, b2, b3, b4, d2, d3, d4) - + d1 * determinant_m3(a2, a3, a4, b2, b3, b4, c2, c3, c4)); return ans; } @@ -1000,38 +997,38 @@ float determinant_m4(float m[][4]) void size_to_mat3(float mat[][3], const float size[3]) { - mat[0][0]= size[0]; - mat[0][1]= 0.0f; - mat[0][2]= 0.0f; - mat[1][1]= size[1]; - mat[1][0]= 0.0f; - mat[1][2]= 0.0f; - mat[2][2]= size[2]; - mat[2][1]= 0.0f; - mat[2][0]= 0.0f; + mat[0][0] = size[0]; + mat[0][1] = 0.0f; + mat[0][2] = 0.0f; + mat[1][1] = size[1]; + mat[1][0] = 0.0f; + mat[1][2] = 0.0f; + mat[2][2] = size[2]; + mat[2][1] = 0.0f; + mat[2][0] = 0.0f; } void size_to_mat4(float mat[][4], const float size[3]) { float tmat[3][3]; - - size_to_mat3(tmat,size); + + size_to_mat3(tmat, size); unit_m4(mat); copy_m4_m3(mat, tmat); } void mat3_to_size(float size[3], float mat[][3]) { - size[0]= len_v3(mat[0]); - size[1]= len_v3(mat[1]); - size[2]= len_v3(mat[2]); + size[0] = len_v3(mat[0]); + size[1] = len_v3(mat[1]); + size[2] = len_v3(mat[2]); } void mat4_to_size(float size[3], float mat[][4]) { - size[0]= len_v3(mat[0]); - size[1]= len_v3(mat[1]); - size[2]= len_v3(mat[2]); + size[0] = len_v3(mat[0]); + size[1] = len_v3(mat[1]); + size[2] = len_v3(mat[2]); } /* this gets the average scale of a matrix, only use when your scaling @@ -1052,10 +1049,9 @@ float mat4_to_scale(float mat[][4]) return mat3_to_scale(tmat); } - void mat3_to_rot_size(float rot[3][3], float size[3], float mat3[3][3]) { - float mat3_n[3][3]; /* mat3 -> normalized, 3x3 */ + float mat3_n[3][3]; /* mat3 -> normalized, 3x3 */ float imat3_n[3][3]; /* mat3 -> normalized & inverted, 3x3 */ /* rotation & scale are linked, we need to create the mat's @@ -1079,14 +1075,14 @@ void mat3_to_rot_size(float rot[3][3], float size[3], float mat3[3][3]) invert_m3_m3(imat3_n, mat3_n); mul_m3_m3m3(mat3, imat3_n, mat3); - size[0]= mat3[0][0]; - size[1]= mat3[1][1]; - size[2]= mat3[2][2]; + size[0] = mat3[0][0]; + size[1] = mat3[1][1]; + size[2] = mat3[2][2]; } void mat4_to_loc_rot_size(float loc[3], float rot[3][3], float size[3], float wmat[][4]) { - float mat3[3][3]; /* wmat -> 3x3 */ + float mat3[3][3]; /* wmat -> 3x3 */ copy_m3_m4(mat3, wmat); mat3_to_rot_size(rot, size, mat3); @@ -1097,33 +1093,33 @@ void mat4_to_loc_rot_size(float loc[3], float rot[3][3], float size[3], float wm void scale_m3_fl(float m[][3], float scale) { - m[0][0]= m[1][1]= m[2][2]= scale; - m[0][1]= m[0][2]= 0.0; - m[1][0]= m[1][2]= 0.0; - m[2][0]= m[2][1]= 0.0; + m[0][0] = m[1][1] = m[2][2] = scale; + m[0][1] = m[0][2] = 0.0; + m[1][0] = m[1][2] = 0.0; + m[2][0] = m[2][1] = 0.0; } void scale_m4_fl(float m[][4], float scale) { - m[0][0]= m[1][1]= m[2][2]= scale; - m[3][3]= 1.0; - m[0][1]= m[0][2]= m[0][3]= 0.0; - m[1][0]= m[1][2]= m[1][3]= 0.0; - m[2][0]= m[2][1]= m[2][3]= 0.0; - m[3][0]= m[3][1]= m[3][2]= 0.0; + m[0][0] = m[1][1] = m[2][2] = scale; + m[3][3] = 1.0; + m[0][1] = m[0][2] = m[0][3] = 0.0; + m[1][0] = m[1][2] = m[1][3] = 0.0; + m[2][0] = m[2][1] = m[2][3] = 0.0; + m[3][0] = m[3][1] = m[3][2] = 0.0; } -void translate_m4(float mat[][4],float Tx, float Ty, float Tz) +void translate_m4(float mat[][4], float Tx, float Ty, float Tz) { - mat[3][0] += (Tx*mat[0][0] + Ty*mat[1][0] + Tz*mat[2][0]); - mat[3][1] += (Tx*mat[0][1] + Ty*mat[1][1] + Tz*mat[2][1]); - mat[3][2] += (Tx*mat[0][2] + Ty*mat[1][2] + Tz*mat[2][2]); + mat[3][0] += (Tx * mat[0][0] + Ty * mat[1][0] + Tz * mat[2][0]); + mat[3][1] += (Tx * mat[0][1] + Ty * mat[1][1] + Tz * mat[2][1]); + mat[3][2] += (Tx * mat[0][2] + Ty * mat[1][2] + Tz * mat[2][2]); } void rotate_m4(float mat[][4], const char axis, const float angle) { int col; - float temp[4]= {0.0f, 0.0f, 0.0f, 0.0f}; + float temp[4] = {0.0f, 0.0f, 0.0f, 0.0f}; float cosine, sine; assert(axis >= 'X' && axis <= 'Z'); @@ -1131,32 +1127,32 @@ void rotate_m4(float mat[][4], const char axis, const float angle) cosine = (float)cos(angle); sine = (float)sin(angle); switch (axis) { - case 'X': - for (col=0 ; col<4 ; col++) - temp[col] = cosine*mat[1][col] + sine*mat[2][col]; - for (col=0 ; col<4 ; col++) { - mat[2][col] = - sine*mat[1][col] + cosine*mat[2][col]; - mat[1][col] = temp[col]; - } - break; - - case 'Y': - for (col=0 ; col<4 ; col++) - temp[col] = cosine*mat[0][col] - sine*mat[2][col]; - for (col=0 ; col<4 ; col++) { - mat[2][col] = sine*mat[0][col] + cosine*mat[2][col]; - mat[0][col] = temp[col]; - } - break; - - case 'Z': - for (col=0 ; col<4 ; col++) - temp[col] = cosine*mat[0][col] + sine*mat[1][col]; - for (col=0 ; col<4 ; col++) { - mat[1][col] = - sine*mat[0][col] + cosine*mat[1][col]; - mat[0][col] = temp[col]; - } - break; + case 'X': + for (col = 0; col < 4; col++) + temp[col] = cosine * mat[1][col] + sine * mat[2][col]; + for (col = 0; col < 4; col++) { + mat[2][col] = -sine * mat[1][col] + cosine * mat[2][col]; + mat[1][col] = temp[col]; + } + break; + + case 'Y': + for (col = 0; col < 4; col++) + temp[col] = cosine * mat[0][col] - sine * mat[2][col]; + for (col = 0; col < 4; col++) { + mat[2][col] = sine * mat[0][col] + cosine * mat[2][col]; + mat[0][col] = temp[col]; + } + break; + + case 'Z': + for (col = 0; col < 4; col++) + temp[col] = cosine * mat[0][col] + sine * mat[1][col]; + for (col = 0; col < 4; col++) { + mat[1][col] = -sine * mat[0][col] + cosine * mat[1][col]; + mat[0][col] = temp[col]; + } + break; } } @@ -1166,7 +1162,7 @@ void blend_m3_m3m3(float out[][3], float dst[][3], float src[][3], const float s float squat[4], dquat[4], fquat[4]; float sscale[3], dscale[3], fsize[3]; float rmat[3][3], smat[3][3]; - + mat3_to_rot_size(drot, dscale, dst); mat3_to_rot_size(srot, sscale, src); @@ -1178,8 +1174,8 @@ void blend_m3_m3m3(float out[][3], float dst[][3], float src[][3], const float s interp_v3_v3v3(fsize, dscale, sscale, srcweight); /* compose new matrix */ - quat_to_mat3(rmat,fquat); - size_to_mat3(smat,fsize); + quat_to_mat3(rmat, fquat); + size_to_mat3(smat, fsize); mul_m3_m3m3(out, rmat, smat); } @@ -1205,7 +1201,6 @@ void blend_m4_m4m4(float out[][4], float dst[][4], float src[][4], const float s loc_quat_size_to_mat4(out, floc, fquat, fsize); } - int is_negative_m3(float mat[][3]) { float vec[3]; @@ -1223,21 +1218,22 @@ int is_negative_m4(float mat[][4]) /* make a 4x4 matrix out of 3 transform components */ /* matrices are made in the order: scale * rot * loc */ // TODO: need to have a version that allows for rotation order... + void loc_eul_size_to_mat4(float mat[4][4], const float loc[3], const float eul[3], const float size[3]) { float rmat[3][3], smat[3][3], tmat[3][3]; - + /* initialize new matrix */ unit_m4(mat); - + /* make rotation + scaling part */ - eul_to_mat3(rmat,eul); - size_to_mat3(smat,size); + eul_to_mat3(rmat, eul); + size_to_mat3(smat, size); mul_m3_m3m3(tmat, rmat, smat); - + /* copy rot/scale part to output matrix*/ copy_m4_m3(mat, tmat); - + /* copy location to matrix */ mat[3][0] = loc[0]; mat[3][1] = loc[1]; @@ -1245,22 +1241,23 @@ void loc_eul_size_to_mat4(float mat[4][4], const float loc[3], const float eul[3 } /* make a 4x4 matrix out of 3 transform components */ + /* matrices are made in the order: scale * rot * loc */ void loc_eulO_size_to_mat4(float mat[4][4], const float loc[3], const float eul[3], const float size[3], const short rotOrder) { float rmat[3][3], smat[3][3], tmat[3][3]; - + /* initialize new matrix */ unit_m4(mat); - + /* make rotation + scaling part */ - eulO_to_mat3(rmat,eul, rotOrder); - size_to_mat3(smat,size); + eulO_to_mat3(rmat, eul, rotOrder); + size_to_mat3(smat, size); mul_m3_m3m3(tmat, rmat, smat); - + /* copy rot/scale part to output matrix*/ copy_m4_m3(mat, tmat); - + /* copy location to matrix */ mat[3][0] = loc[0]; mat[3][1] = loc[1]; @@ -1269,22 +1266,23 @@ void loc_eulO_size_to_mat4(float mat[4][4], const float loc[3], const float eul[ /* make a 4x4 matrix out of 3 transform components */ + /* matrices are made in the order: scale * rot * loc */ void loc_quat_size_to_mat4(float mat[4][4], const float loc[3], const float quat[4], const float size[3]) { float rmat[3][3], smat[3][3], tmat[3][3]; - + /* initialize new matrix */ unit_m4(mat); - + /* make rotation + scaling part */ - quat_to_mat3(rmat,quat); - size_to_mat3(smat,size); + quat_to_mat3(rmat, quat); + size_to_mat3(smat, size); mul_m3_m3m3(tmat, rmat, smat); - + /* copy rot/scale part to output matrix*/ copy_m4_m3(mat, tmat); - + /* copy location to matrix */ mat[3][0] = loc[0]; mat[3][1] = loc[1]; @@ -1303,19 +1301,19 @@ void loc_axisangle_size_to_mat4(float mat[4][4], const float loc[3], const float void print_m3(const char *str, float m[][3]) { printf("%s\n", str); - printf("%f %f %f\n",m[0][0],m[1][0],m[2][0]); - printf("%f %f %f\n",m[0][1],m[1][1],m[2][1]); - printf("%f %f %f\n",m[0][2],m[1][2],m[2][2]); + printf("%f %f %f\n", m[0][0], m[1][0], m[2][0]); + printf("%f %f %f\n", m[0][1], m[1][1], m[2][1]); + printf("%f %f %f\n", m[0][2], m[1][2], m[2][2]); printf("\n"); } void print_m4(const char *str, float m[][4]) { printf("%s\n", str); - printf("%f %f %f %f\n",m[0][0],m[1][0],m[2][0],m[3][0]); - printf("%f %f %f %f\n",m[0][1],m[1][1],m[2][1],m[3][1]); - printf("%f %f %f %f\n",m[0][2],m[1][2],m[2][2],m[3][2]); - printf("%f %f %f %f\n",m[0][3],m[1][3],m[2][3],m[3][3]); + printf("%f %f %f %f\n", m[0][0], m[1][0], m[2][0], m[3][0]); + printf("%f %f %f %f\n", m[0][1], m[1][1], m[2][1], m[3][1]); + printf("%f %f %f %f\n", m[0][2], m[1][2], m[2][2], m[3][2]); + printf("%f %f %f %f\n", m[0][3], m[1][3], m[2][3], m[3][3]); printf("\n"); } @@ -1323,9 +1321,9 @@ void print_m4(const char *str, float m[][4]) * from TNT matrix library * * Compute the Single Value Decomposition of an arbitrary matrix A - * That is compute the 3 matrices U,W,V with U column orthogonal (m,n) - * ,W a diagonal matrix and V an orthogonal square matrix s.t. - * A = U.W.Vt. From this decomposition it is trivial to compute the + * That is compute the 3 matrices U,W,V with U column orthogonal (m,n) + * ,W a diagonal matrix and V an orthogonal square matrix s.t. + * A = U.W.Vt. From this decomposition it is trivial to compute the * (pseudo-inverse) of A as Ainv = V.Winv.tranpose(U). */ @@ -1336,25 +1334,25 @@ void svd_m4(float U[4][4], float s[4], float V[4][4], float A_[4][4]) int m = 4; int n = 4; int maxiter = 200; - int nu = minf(m,n); + int nu = minf(m, n); float *work = work1; float *e = work2; float eps; - int i=0, j=0, k=0, p, pp, iter; + int i = 0, j = 0, k = 0, p, pp, iter; // Reduce A to bidiagonal form, storing the diagonal elements // in s and the super-diagonal elements in e. - int nct = minf(m-1,n); - int nrt = maxf(0,minf(n-2,m)); + int nct = minf(m - 1, n); + int nrt = maxf(0, minf(n - 2, m)); copy_m4_m4(A, A_); zero_m4(U); zero_v4(s); - for (k = 0; k < maxf(nct,nrt); k++) { + for (k = 0; k < maxf(nct, nrt); k++) { if (k < nct) { // Compute the transformation for the k-th column and @@ -1362,14 +1360,14 @@ void svd_m4(float U[4][4], float s[4], float V[4][4], float A_[4][4]) // Compute 2-norm of k-th column without under/overflow. s[k] = 0; for (i = k; i < m; i++) { - s[k] = hypotf(s[k],A[i][k]); + s[k] = hypotf(s[k], A[i][k]); } if (s[k] != 0.0f) { float invsk; if (A[k][k] < 0.0f) { s[k] = -s[k]; } - invsk = 1.0f/s[k]; + invsk = 1.0f / s[k]; for (i = k; i < m; i++) { A[i][k] *= invsk; } @@ -1377,18 +1375,18 @@ void svd_m4(float U[4][4], float s[4], float V[4][4], float A_[4][4]) } s[k] = -s[k]; } - for (j = k+1; j < n; j++) { + for (j = k + 1; j < n; j++) { if ((k < nct) && (s[k] != 0.0f)) { - // Apply the transformation. + // Apply the transformation. float t = 0; for (i = k; i < m; i++) { - t += A[i][k]*A[i][j]; + t += A[i][k] * A[i][j]; } - t = -t/A[k][k]; + t = -t / A[k][k]; for (i = k; i < m; i++) { - A[i][j] += t*A[i][k]; + A[i][j] += t * A[i][k]; } } @@ -1411,39 +1409,39 @@ void svd_m4(float U[4][4], float s[4], float V[4][4], float A_[4][4]) // k-th super-diagonal in e[k]. // Compute 2-norm without under/overflow. e[k] = 0; - for (i = k+1; i < n; i++) { - e[k] = hypotf(e[k],e[i]); + for (i = k + 1; i < n; i++) { + e[k] = hypotf(e[k], e[i]); } if (e[k] != 0.0f) { float invek; - if (e[k+1] < 0.0f) { + if (e[k + 1] < 0.0f) { e[k] = -e[k]; } - invek = 1.0f/e[k]; - for (i = k+1; i < n; i++) { + invek = 1.0f / e[k]; + for (i = k + 1; i < n; i++) { e[i] *= invek; } - e[k+1] += 1.0f; + e[k + 1] += 1.0f; } e[k] = -e[k]; - if ((k+1 < m) & (e[k] != 0.0f)) { + if ((k + 1 < m) & (e[k] != 0.0f)) { float invek1; - // Apply the transformation. + // Apply the transformation. - for (i = k+1; i < m; i++) { + for (i = k + 1; i < m; i++) { work[i] = 0.0f; } - for (j = k+1; j < n; j++) { - for (i = k+1; i < m; i++) { - work[i] += e[j]*A[i][j]; + for (j = k + 1; j < n; j++) { + for (i = k + 1; i < m; i++) { + work[i] += e[j] * A[i][j]; } } - invek1 = 1.0f/e[k+1]; - for (j = k+1; j < n; j++) { - float t = -e[j]*invek1; - for (i = k+1; i < m; i++) { - A[i][j] += t*work[i]; + invek1 = 1.0f / e[k + 1]; + for (j = k + 1; j < n; j++) { + float t = -e[j] * invek1; + for (i = k + 1; i < m; i++) { + A[i][j] += t * work[i]; } } } @@ -1451,24 +1449,24 @@ void svd_m4(float U[4][4], float s[4], float V[4][4], float A_[4][4]) // Place the transformation in V for subsequent // back multiplication. - for (i = k+1; i < n; i++) + for (i = k + 1; i < n; i++) V[i][k] = e[i]; } } // Set up the final bidiagonal matrix or order p. - p = minf(n,m+1); + p = minf(n, m + 1); if (nct < n) { s[nct] = A[nct][nct]; } if (m < p) { - s[p-1] = 0.0f; + s[p - 1] = 0.0f; } - if (nrt+1 < p) { - e[nrt] = A[nrt][p-1]; + if (nrt + 1 < p) { + e[nrt] = A[nrt][p - 1]; } - e[p-1] = 0.0f; + e[p - 1] = 0.0f; // If required, generate U. @@ -1478,23 +1476,23 @@ void svd_m4(float U[4][4], float s[4], float V[4][4], float A_[4][4]) } U[j][j] = 1.0f; } - for (k = nct-1; k >= 0; k--) { + for (k = nct - 1; k >= 0; k--) { if (s[k] != 0.0f) { - for (j = k+1; j < nu; j++) { + for (j = k + 1; j < nu; j++) { float t = 0; for (i = k; i < m; i++) { - t += U[i][k]*U[i][j]; + t += U[i][k] * U[i][j]; } - t = -t/U[k][k]; + t = -t / U[k][k]; for (i = k; i < m; i++) { - U[i][j] += t*U[i][k]; + U[i][j] += t * U[i][k]; } } - for (i = k; i < m; i++ ) { + for (i = k; i < m; i++) { U[i][k] = -U[i][k]; } U[k][k] = 1.0f + U[k][k]; - for (i = 0; i < k-1; i++) { + for (i = 0; i < k - 1; i++) { U[i][k] = 0.0f; } } @@ -1508,16 +1506,16 @@ void svd_m4(float U[4][4], float s[4], float V[4][4], float A_[4][4]) // If required, generate V. - for (k = n-1; k >= 0; k--) { + for (k = n - 1; k >= 0; k--) { if ((k < nrt) & (e[k] != 0.0f)) { - for (j = k+1; j < nu; j++) { + for (j = k + 1; j < nu; j++) { float t = 0; - for (i = k+1; i < n; i++) { - t += V[i][k]*V[i][j]; + for (i = k + 1; i < n; i++) { + t += V[i][k] * V[i][j]; } - t = -t/V[k+1][k]; - for (i = k+1; i < n; i++) { - V[i][j] += t*V[i][k]; + t = -t / V[k + 1][k]; + for (i = k + 1; i < n; i++) { + V[i][j] += t * V[i][k]; } } } @@ -1529,11 +1527,11 @@ void svd_m4(float U[4][4], float s[4], float V[4][4], float A_[4][4]) // Main iteration loop for the singular values. - pp = p-1; + pp = p - 1; iter = 0; - eps = powf(2.0f,-52.0f); + eps = powf(2.0f, -52.0f); while (p > 0) { - int kase=0; + int kase = 0; // Test for maximum iterations to avoid infinite loop if (maxiter == 0) @@ -1544,34 +1542,34 @@ void svd_m4(float U[4][4], float s[4], float V[4][4], float A_[4][4]) // negligible elements in the s and e arrays. On // completion the variables kase and k are set as follows. - // kase = 1 if s(p) and e[k-1] are negligible and k<p + // kase = 1 if s(p) and e[k - 1] are negligible and k<p // kase = 2 if s(k) is negligible and k<p - // kase = 3 if e[k-1] is negligible, k<p, and - // s(k), ..., s(p) are not negligible (qr step). - // kase = 4 if e(p-1) is negligible (convergence). + // kase = 3 if e[k - 1] is negligible, k<p, and + // s(k), ..., s(p) are not negligible (qr step). + // kase = 4 if e(p - 1) is negligible (convergence). - for (k = p-2; k >= -1; k--) { + for (k = p - 2; k >= -1; k--) { if (k == -1) { break; } - if (fabsf(e[k]) <= eps*(fabsf(s[k]) + fabsf(s[k+1]))) { + if (fabsf(e[k]) <= eps * (fabsf(s[k]) + fabsf(s[k + 1]))) { e[k] = 0.0f; break; } } - if (k == p-2) { + if (k == p - 2) { kase = 4; } else { int ks; - for (ks = p-1; ks >= k; ks--) { + for (ks = p - 1; ks >= k; ks--) { float t; if (ks == k) { break; } - t = (ks != p ? fabsf(e[ks]) : 0.f) + - (ks != k+1 ? fabsf(e[ks-1]) : 0.0f); - if (fabsf(s[ks]) <= eps*t) { + t = (ks != p ? fabsf(e[ks]) : 0.f) + + (ks != k + 1 ? fabsf(e[ks - 1]) : 0.0f); + if (fabsf(s[ks]) <= eps * t) { s[ks] = 0.0f; break; } @@ -1579,7 +1577,7 @@ void svd_m4(float U[4][4], float s[4], float V[4][4], float A_[4][4]) if (ks == k) { kase = 3; } - else if (ks == p-1) { + else if (ks == p - 1) { kase = 1; } else { @@ -1595,127 +1593,130 @@ void svd_m4(float U[4][4], float s[4], float V[4][4], float A_[4][4]) // Deflate negligible s(p). - case 1: { - float f = e[p-2]; - e[p-2] = 0.0f; - for (j = p-2; j >= k; j--) { - float t = hypotf(s[j],f); - float invt = 1.0f/t; - float cs = s[j]*invt; - float sn = f*invt; + case 1: + { + float f = e[p - 2]; + e[p - 2] = 0.0f; + for (j = p - 2; j >= k; j--) { + float t = hypotf(s[j], f); + float invt = 1.0f / t; + float cs = s[j] * invt; + float sn = f * invt; s[j] = t; if (j != k) { - f = -sn*e[j-1]; - e[j-1] = cs*e[j-1]; + f = -sn * e[j - 1]; + e[j - 1] = cs * e[j - 1]; } for (i = 0; i < n; i++) { - t = cs*V[i][j] + sn*V[i][p-1]; - V[i][p-1] = -sn*V[i][j] + cs*V[i][p-1]; + t = cs * V[i][j] + sn * V[i][p - 1]; + V[i][p - 1] = -sn * V[i][j] + cs * V[i][p - 1]; V[i][j] = t; } } + break; } - break; - // Split at negligible s(k). + // Split at negligible s(k). - case 2: { - float f = e[k-1]; - e[k-1] = 0.0f; + case 2: + { + float f = e[k - 1]; + e[k - 1] = 0.0f; for (j = k; j < p; j++) { - float t = hypotf(s[j],f); - float invt = 1.0f/t; - float cs = s[j]*invt; - float sn = f*invt; + float t = hypotf(s[j], f); + float invt = 1.0f / t; + float cs = s[j] * invt; + float sn = f * invt; s[j] = t; - f = -sn*e[j]; - e[j] = cs*e[j]; + f = -sn * e[j]; + e[j] = cs * e[j]; for (i = 0; i < m; i++) { - t = cs*U[i][j] + sn*U[i][k-1]; - U[i][k-1] = -sn*U[i][j] + cs*U[i][k-1]; + t = cs * U[i][j] + sn * U[i][k - 1]; + U[i][k - 1] = -sn * U[i][j] + cs * U[i][k - 1]; U[i][j] = t; } } + break; } - break; - // Perform one qr step. + // Perform one qr step. - case 3: { + case 3: + { // Calculate the shift. float scale = maxf(maxf(maxf(maxf( - fabsf(s[p-1]),fabsf(s[p-2])),fabsf(e[p-2])), - fabsf(s[k])),fabsf(e[k])); - float invscale = 1.0f/scale; - float sp = s[p-1]*invscale; - float spm1 = s[p-2]*invscale; - float epm1 = e[p-2]*invscale; - float sk = s[k]*invscale; - float ek = e[k]*invscale; - float b = ((spm1 + sp)*(spm1 - sp) + epm1*epm1)*0.5f; - float c = (sp*epm1)*(sp*epm1); + fabsf(s[p - 1]),fabsf(s[p - 2])),fabsf(e[p - 2])), + fabsf(s[k])),fabsf(e[k])); + float invscale = 1.0f / scale; + float sp = s[p - 1] * invscale; + float spm1 = s[p - 2] * invscale; + float epm1 = e[p - 2] * invscale; + float sk = s[k] * invscale; + float ek = e[k] * invscale; + float b = ((spm1 + sp) * (spm1 - sp) + epm1 * epm1) * 0.5f; + float c = (sp * epm1) * (sp * epm1); float shift = 0.0f; float f, g; if ((b != 0.0f) || (c != 0.0f)) { - shift = sqrtf(b*b + c); + shift = sqrtf(b * b + c); if (b < 0.0f) { shift = -shift; } - shift = c/(b + shift); + shift = c / (b + shift); } - f = (sk + sp)*(sk - sp) + shift; - g = sk*ek; + f = (sk + sp) * (sk - sp) + shift; + g = sk * ek; // Chase zeros. - for (j = k; j < p-1; j++) { - float t = hypotf(f,g); + for (j = k; j < p - 1; j++) { + float t = hypotf(f, g); /* division by zero checks added to avoid NaN (brecht) */ - float cs = (t == 0.0f)? 0.0f: f/t; - float sn = (t == 0.0f)? 0.0f: g/t; + float cs = (t == 0.0f) ? 0.0f : f / t; + float sn = (t == 0.0f) ? 0.0f : g / t; if (j != k) { - e[j-1] = t; + e[j - 1] = t; } - f = cs*s[j] + sn*e[j]; - e[j] = cs*e[j] - sn*s[j]; - g = sn*s[j+1]; - s[j+1] = cs*s[j+1]; + f = cs * s[j] + sn * e[j]; + e[j] = cs * e[j] - sn * s[j]; + g = sn * s[j + 1]; + s[j + 1] = cs * s[j + 1]; for (i = 0; i < n; i++) { - t = cs*V[i][j] + sn*V[i][j+1]; - V[i][j+1] = -sn*V[i][j] + cs*V[i][j+1]; + t = cs * V[i][j] + sn * V[i][j + 1]; + V[i][j + 1] = -sn * V[i][j] + cs * V[i][j + 1]; V[i][j] = t; } - t = hypotf(f,g); + t = hypotf(f, g); /* division by zero checks added to avoid NaN (brecht) */ - cs = (t == 0.0f)? 0.0f: f/t; - sn = (t == 0.0f)? 0.0f: g/t; + cs = (t == 0.0f) ? 0.0f : f / t; + sn = (t == 0.0f) ? 0.0f : g / t; s[j] = t; - f = cs*e[j] + sn*s[j+1]; - s[j+1] = -sn*e[j] + cs*s[j+1]; - g = sn*e[j+1]; - e[j+1] = cs*e[j+1]; - if (j < m-1) { + f = cs * e[j] + sn * s[j + 1]; + s[j + 1] = -sn * e[j] + cs * s[j + 1]; + g = sn * e[j + 1]; + e[j + 1] = cs * e[j + 1]; + if (j < m - 1) { for (i = 0; i < m; i++) { - t = cs*U[i][j] + sn*U[i][j+1]; - U[i][j+1] = -sn*U[i][j] + cs*U[i][j+1]; + t = cs * U[i][j] + sn * U[i][j + 1]; + U[i][j + 1] = -sn * U[i][j] + cs * U[i][j + 1]; U[i][j] = t; } } } - e[p-2] = f; + e[p - 2] = f; iter = iter + 1; + break; } - break; - - // Convergence. + // Convergence. - case 4: { + case 4: + { // Make the singular values positive. @@ -1730,28 +1731,32 @@ void svd_m4(float U[4][4], float s[4], float V[4][4], float A_[4][4]) while (k < pp) { float t; - if (s[k] >= s[k+1]) { + if (s[k] >= s[k + 1]) { break; } t = s[k]; - s[k] = s[k+1]; - s[k+1] = t; - if (k < n-1) { + s[k] = s[k + 1]; + s[k + 1] = t; + if (k < n - 1) { for (i = 0; i < n; i++) { - t = V[i][k+1]; V[i][k+1] = V[i][k]; V[i][k] = t; + t = V[i][k + 1]; + V[i][k + 1] = V[i][k]; + V[i][k] = t; } } - if (k < m-1) { + if (k < m - 1) { for (i = 0; i < m; i++) { - t = U[i][k+1]; U[i][k+1] = U[i][k]; U[i][k] = t; + t = U[i][k + 1]; + U[i][k + 1] = U[i][k]; + U[i][k] = t; } } k++; } iter = 0; p--; + break; } - break; } } } @@ -1769,8 +1774,8 @@ void pseudoinverse_m4_m4(float Ainv[4][4], float A[4][4], float epsilon) transpose_m4(V); zero_m4(Wm); - for (i=0; i<4; i++) - Wm[i][i]= (W[i] < epsilon)? 0.0f: 1.0f/W[i]; + for (i = 0; i < 4; i++) + Wm[i][i] = (W[i] < epsilon) ? 0.0f : 1.0f / W[i]; transpose_m4(V); diff --git a/source/blender/blenlib/intern/math_rotation.c b/source/blender/blenlib/intern/math_rotation.c index 5ae226bf6d5..25e7e451451 100644 --- a/source/blender/blenlib/intern/math_rotation.c +++ b/source/blender/blenlib/intern/math_rotation.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 ***** @@ -40,25 +40,24 @@ /* convenience, avoids setting Y axis everywhere */ void unit_axis_angle(float axis[3], float *angle) { - axis[0]= 0.0f; - axis[1]= 1.0f; - axis[2]= 0.0f; - *angle= 0.0f; + axis[0] = 0.0f; + axis[1] = 1.0f; + axis[2] = 0.0f; + *angle = 0.0f; } - void unit_qt(float q[4]) { - q[0]= 1.0f; - q[1]= q[2]= q[3]= 0.0f; + q[0] = 1.0f; + q[1] = q[2] = q[3] = 0.0f; } void copy_qt_qt(float q1[4], const float q2[4]) { - q1[0]= q2[0]; - q1[1]= q2[1]; - q1[2]= q2[2]; - q1[3]= q2[3]; + q1[0] = q2[0]; + q1[1] = q2[1]; + q1[2] = q2[2]; + q1[3] = q2[3]; } int is_zero_qt(float *q) @@ -66,17 +65,17 @@ int is_zero_qt(float *q) return (q[0] == 0 && q[1] == 0 && q[2] == 0 && q[3] == 0); } -void mul_qt_qtqt(float *q, const float *q1, const float *q2) +void mul_qt_qtqt(float q[4], const float q1[4], const float q2[4]) { - float t0,t1,t2; + float t0, t1, t2; - t0= q1[0]*q2[0]-q1[1]*q2[1]-q1[2]*q2[2]-q1[3]*q2[3]; - t1= q1[0]*q2[1]+q1[1]*q2[0]+q1[2]*q2[3]-q1[3]*q2[2]; - t2= q1[0]*q2[2]+q1[2]*q2[0]+q1[3]*q2[1]-q1[1]*q2[3]; - q[3]= q1[0]*q2[3]+q1[3]*q2[0]+q1[1]*q2[2]-q1[2]*q2[1]; - q[0]=t0; - q[1]=t1; - q[2]=t2; + t0 = q1[0] * q2[0] - q1[1] * q2[1] - q1[2] * q2[2] - q1[3] * q2[3]; + t1 = q1[0] * q2[1] + q1[1] * q2[0] + q1[2] * q2[3] - q1[3] * q2[2]; + t2 = q1[0] * q2[2] + q1[2] * q2[0] + q1[3] * q2[1] - q1[1] * q2[3]; + q[3] = q1[0] * q2[3] + q1[3] * q2[0] + q1[1] * q2[2] - q1[2] * q2[1]; + q[0] = t0; + q[1] = t1; + q[2] = t2; } /* Assumes a unit quaternion */ @@ -84,18 +83,18 @@ void mul_qt_v3(const float q[4], float v[3]) { float t0, t1, t2; - t0= -q[1]*v[0]-q[2]*v[1]-q[3]*v[2]; - t1= q[0]*v[0]+q[2]*v[2]-q[3]*v[1]; - t2= q[0]*v[1]+q[3]*v[0]-q[1]*v[2]; - v[2]= q[0]*v[2]+q[1]*v[1]-q[2]*v[0]; - v[0]=t1; - v[1]=t2; + t0 = -q[1] * v[0] - q[2] * v[1] - q[3] * v[2]; + t1 = q[0] * v[0] + q[2] * v[2] - q[3] * v[1]; + t2 = q[0] * v[1] + q[3] * v[0] - q[1] * v[2]; + v[2] = q[0] * v[2] + q[1] * v[1] - q[2] * v[0]; + v[0] = t1; + v[1] = t2; - t1= t0*-q[1]+v[0]*q[0]-v[1]*q[3]+v[2]*q[2]; - t2= t0*-q[2]+v[1]*q[0]-v[2]*q[1]+v[0]*q[3]; - v[2]= t0*-q[3]+v[2]*q[0]-v[0]*q[2]+v[1]*q[1]; - v[0]=t1; - v[1]=t2; + t1 = t0 * -q[1] + v[0] * q[0] - v[1] * q[3] + v[2] * q[2]; + t2 = t0 * -q[2] + v[1] * q[0] - v[2] * q[1] + v[0] * q[3]; + v[2] = t0 * -q[3] + v[2] * q[0] - v[0] * q[2] + v[1] * q[1]; + v[0] = t1; + v[1] = t2; } void conjugate_qt(float q[4]) @@ -107,10 +106,10 @@ void conjugate_qt(float q[4]) float dot_qtqt(const float q1[4], const float q2[4]) { - return q1[0]*q2[0] + q1[1]*q2[1] + q1[2]*q2[2] + q1[3]*q2[3]; + return q1[0] * q2[0] + q1[1] * q2[1] + q1[2] * q2[2] + q1[3] * q2[3]; } -void invert_qt(float *q) +void invert_qt(float q[4]) { float f = dot_qtqt(q, q); @@ -118,17 +117,17 @@ void invert_qt(float *q) return; conjugate_qt(q); - mul_qt_fl(q, 1.0f/f); + mul_qt_fl(q, 1.0f / f); } -void invert_qt_qt(float *q1, const float *q2) +void invert_qt_qt(float q1[4], const float q2[4]) { copy_qt_qt(q1, q2); invert_qt(q1); } /* simple mult */ -void mul_qt_fl(float *q, const float f) +void mul_qt_fl(float q[4], const float f) { q[0] *= f; q[1] *= f; @@ -140,65 +139,64 @@ void sub_qt_qtqt(float q[4], const float q1[4], const float q2[4]) { float nq2[4]; - nq2[0]= -q2[0]; - nq2[1]= q2[1]; - nq2[2]= q2[2]; - nq2[3]= q2[3]; + nq2[0] = -q2[0]; + nq2[1] = q2[1]; + nq2[2] = q2[2]; + nq2[3] = q2[3]; mul_qt_qtqt(q, q1, nq2); } /* angular mult factor */ -void mul_fac_qt_fl(float *q, const float fac) +void mul_fac_qt_fl(float q[4], const float fac) { - float angle= fac*saacos(q[0]); /* quat[0]= cos(0.5*angle), but now the 0.5 and 2.0 rule out */ - - float co= (float)cos(angle); - float si= (float)sin(angle); - q[0]= co; - normalize_v3(q+1); - mul_v3_fl(q+1, si); + float angle = fac * saacos(q[0]); /* quat[0] = cos(0.5 * angle), but now the 0.5 and 2.0 rule out */ + + float co = (float)cos(angle); + float si = (float)sin(angle); + q[0] = co; + normalize_v3(q + 1); + mul_v3_fl(q + 1, si); } /* skip error check, currently only needed by mat3_to_quat_is_ok */ static void quat_to_mat3_no_error(float m[][3], const float q[4]) { - double q0, q1, q2, q3, qda,qdb,qdc,qaa,qab,qac,qbb,qbc,qcc; + double q0, q1, q2, q3, qda, qdb, qdc, qaa, qab, qac, qbb, qbc, qcc; - q0= M_SQRT2 * (double)q[0]; - q1= M_SQRT2 * (double)q[1]; - q2= M_SQRT2 * (double)q[2]; - q3= M_SQRT2 * (double)q[3]; + q0 = M_SQRT2 * (double)q[0]; + q1 = M_SQRT2 * (double)q[1]; + q2 = M_SQRT2 * (double)q[2]; + q3 = M_SQRT2 * (double)q[3]; - qda= q0*q1; - qdb= q0*q2; - qdc= q0*q3; - qaa= q1*q1; - qab= q1*q2; - qac= q1*q3; - qbb= q2*q2; - qbc= q2*q3; - qcc= q3*q3; + qda = q0 * q1; + qdb = q0 * q2; + qdc = q0 * q3; + qaa = q1 * q1; + qab = q1 * q2; + qac = q1 * q3; + qbb = q2 * q2; + qbc = q2 * q3; + qcc = q3 * q3; - m[0][0]= (float)(1.0-qbb-qcc); - m[0][1]= (float)(qdc+qab); - m[0][2]= (float)(-qdb+qac); + m[0][0] = (float)(1.0 - qbb - qcc); + m[0][1] = (float)(qdc + qab); + m[0][2] = (float)(-qdb + qac); - m[1][0]= (float)(-qdc+qab); - m[1][1]= (float)(1.0-qaa-qcc); - m[1][2]= (float)(qda+qbc); + m[1][0] = (float)(-qdc + qab); + m[1][1] = (float)(1.0 - qaa - qcc); + m[1][2] = (float)(qda + qbc); - m[2][0]= (float)(qdb+qac); - m[2][1]= (float)(-qda+qbc); - m[2][2]= (float)(1.0-qaa-qbb); + m[2][0] = (float)(qdb + qac); + m[2][1] = (float)(-qda + qbc); + m[2][2] = (float)(1.0 - qaa - qbb); } - void quat_to_mat3(float m[][3], const float q[4]) { #ifdef DEBUG float f; - if (!((f=dot_qtqt(q, q))==0.0f || (fabsf(f-1.0f) < (float)QUAT_EPSILON))) { + if (!((f = dot_qtqt(q, q)) == 0.0f || (fabsf(f - 1.0f) < (float)QUAT_EPSILON))) { fprintf(stderr, "Warning! quat_to_mat3() called with non-normalized: size %.8f *** report a bug ***\n", f); } #endif @@ -208,106 +206,106 @@ void quat_to_mat3(float m[][3], const float q[4]) void quat_to_mat4(float m[][4], const float q[4]) { - double q0, q1, q2, q3, qda,qdb,qdc,qaa,qab,qac,qbb,qbc,qcc; + double q0, q1, q2, q3, qda, qdb, qdc, qaa, qab, qac, qbb, qbc, qcc; #ifdef DEBUG - if (!((q0=dot_qtqt(q, q))==0.0f || (fabsf(q0-1.0) < QUAT_EPSILON))) { + if (!((q0 = dot_qtqt(q, q)) == 0.0f || (fabsf(q0 - 1.0) < QUAT_EPSILON))) { fprintf(stderr, "Warning! quat_to_mat4() called with non-normalized: size %.8f *** report a bug ***\n", (float)q0); } #endif - q0= M_SQRT2 * (double)q[0]; - q1= M_SQRT2 * (double)q[1]; - q2= M_SQRT2 * (double)q[2]; - q3= M_SQRT2 * (double)q[3]; - - qda= q0*q1; - qdb= q0*q2; - qdc= q0*q3; - qaa= q1*q1; - qab= q1*q2; - qac= q1*q3; - qbb= q2*q2; - qbc= q2*q3; - qcc= q3*q3; - - m[0][0]= (float)(1.0-qbb-qcc); - m[0][1]= (float)(qdc+qab); - m[0][2]= (float)(-qdb+qac); - m[0][3]= 0.0f; - - m[1][0]= (float)(-qdc+qab); - m[1][1]= (float)(1.0-qaa-qcc); - m[1][2]= (float)(qda+qbc); - m[1][3]= 0.0f; - - m[2][0]= (float)(qdb+qac); - m[2][1]= (float)(-qda+qbc); - m[2][2]= (float)(1.0-qaa-qbb); - m[2][3]= 0.0f; - - m[3][0]= m[3][1]= m[3][2]= 0.0f; - m[3][3]= 1.0f; -} - -void mat3_to_quat(float *q, float wmat[][3]) + q0 = M_SQRT2 * (double)q[0]; + q1 = M_SQRT2 * (double)q[1]; + q2 = M_SQRT2 * (double)q[2]; + q3 = M_SQRT2 * (double)q[3]; + + qda = q0 * q1; + qdb = q0 * q2; + qdc = q0 * q3; + qaa = q1 * q1; + qab = q1 * q2; + qac = q1 * q3; + qbb = q2 * q2; + qbc = q2 * q3; + qcc = q3 * q3; + + m[0][0] = (float)(1.0 - qbb - qcc); + m[0][1] = (float)(qdc + qab); + m[0][2] = (float)(-qdb + qac); + m[0][3] = 0.0f; + + m[1][0] = (float)(-qdc + qab); + m[1][1] = (float)(1.0 - qaa - qcc); + m[1][2] = (float)(qda + qbc); + m[1][3] = 0.0f; + + m[2][0] = (float)(qdb + qac); + m[2][1] = (float)(-qda + qbc); + m[2][2] = (float)(1.0 - qaa - qbb); + m[2][3] = 0.0f; + + m[3][0] = m[3][1] = m[3][2] = 0.0f; + m[3][3] = 1.0f; +} + +void mat3_to_quat(float q[4], float wmat[][3]) { double tr, s; float mat[3][3]; /* work on a copy */ copy_m3_m3(mat, wmat); - normalize_m3(mat); /* this is needed AND a 'normalize_qt' in the end */ - - tr= 0.25* (double)(1.0f+mat[0][0]+mat[1][1]+mat[2][2]); - - if (tr>(double)FLT_EPSILON) { - s= sqrt(tr); - q[0]= (float)s; - s= 1.0/(4.0*s); - q[1]= (float)((mat[1][2]-mat[2][1])*s); - q[2]= (float)((mat[2][0]-mat[0][2])*s); - q[3]= (float)((mat[0][1]-mat[1][0])*s); + normalize_m3(mat); /* this is needed AND a 'normalize_qt' in the end */ + + tr = 0.25 * (double)(1.0f + mat[0][0] + mat[1][1] + mat[2][2]); + + if (tr > (double)FLT_EPSILON) { + s = sqrt(tr); + q[0] = (float)s; + s = 1.0 / (4.0 * s); + q[1] = (float)((mat[1][2] - mat[2][1]) * s); + q[2] = (float)((mat[2][0] - mat[0][2]) * s); + q[3] = (float)((mat[0][1] - mat[1][0]) * s); } else { if (mat[0][0] > mat[1][1] && mat[0][0] > mat[2][2]) { - s= 2.0f*sqrtf(1.0f + mat[0][0] - mat[1][1] - mat[2][2]); - q[1]= (float)(0.25*s); + s = 2.0f * sqrtf(1.0f + mat[0][0] - mat[1][1] - mat[2][2]); + q[1] = (float)(0.25 * s); - s= 1.0/s; - q[0]= (float)((double)(mat[2][1] - mat[1][2])*s); - q[2]= (float)((double)(mat[1][0] + mat[0][1])*s); - q[3]= (float)((double)(mat[2][0] + mat[0][2])*s); + s = 1.0 / s; + q[0] = (float)((double)(mat[2][1] - mat[1][2]) * s); + q[2] = (float)((double)(mat[1][0] + mat[0][1]) * s); + q[3] = (float)((double)(mat[2][0] + mat[0][2]) * s); } else if (mat[1][1] > mat[2][2]) { - s= 2.0f*sqrtf(1.0f + mat[1][1] - mat[0][0] - mat[2][2]); - q[2]= (float)(0.25*s); + s = 2.0f * sqrtf(1.0f + mat[1][1] - mat[0][0] - mat[2][2]); + q[2] = (float)(0.25 * s); - s= 1.0/s; - q[0]= (float)((double)(mat[2][0] - mat[0][2])*s); - q[1]= (float)((double)(mat[1][0] + mat[0][1])*s); - q[3]= (float)((double)(mat[2][1] + mat[1][2])*s); + s = 1.0 / s; + q[0] = (float)((double)(mat[2][0] - mat[0][2]) * s); + q[1] = (float)((double)(mat[1][0] + mat[0][1]) * s); + q[3] = (float)((double)(mat[2][1] + mat[1][2]) * s); } else { - s= 2.0f*sqrtf(1.0f + mat[2][2] - mat[0][0] - mat[1][1]); - q[3]= (float)(0.25*s); + s = 2.0f * sqrtf(1.0f + mat[2][2] - mat[0][0] - mat[1][1]); + q[3] = (float)(0.25 * s); - s= 1.0/s; - q[0]= (float)((double)(mat[1][0] - mat[0][1])*s); - q[1]= (float)((double)(mat[2][0] + mat[0][2])*s); - q[2]= (float)((double)(mat[2][1] + mat[1][2])*s); + s = 1.0 / s; + q[0] = (float)((double)(mat[1][0] - mat[0][1]) * s); + q[1] = (float)((double)(mat[2][0] + mat[0][2]) * s); + q[2] = (float)((double)(mat[2][1] + mat[1][2]) * s); } } normalize_qt(q); } -void mat4_to_quat(float *q, float m[][4]) +void mat4_to_quat(float q[4], float m[][4]) { float mat[3][3]; - + copy_m3_m4(mat, m); - mat3_to_quat(q,mat); + mat3_to_quat(q, mat); } void mat3_to_quat_is_ok(float q[4], float wmat[3][3]) @@ -317,54 +315,53 @@ void mat3_to_quat_is_ok(float q[4], float wmat[3][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[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; + + 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_no_error( matr,q1); + quat_to_mat3_no_error(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; - + 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); } - -float normalize_qt(float *q) +float normalize_qt(float q[4]) { float len; - - len= (float)sqrt(dot_qtqt(q, q)); - if (len!=0.0f) { - mul_qt_fl(q, 1.0f/len); + + len = (float)sqrt(dot_qtqt(q, q)); + if (len != 0.0f) { + mul_qt_fl(q, 1.0f / len); } else { - q[1]= 1.0f; - q[0]= q[2]= q[3]= 0.0f; + q[1] = 1.0f; + q[0] = q[2] = q[3] = 0.0f; } return len; @@ -377,19 +374,19 @@ float normalize_qt_qt(float r[4], const float q[4]) } /* note: expects vectors to be normalized */ -void rotation_between_vecs_to_quat(float *q, const float v1[3], const float v2[3]) +void rotation_between_vecs_to_quat(float q[4], const float v1[3], const float v2[3]) { float axis[3]; float angle; - + cross_v3_v3v3(axis, v1, v2); - + angle = angle_normalized_v3v3(v1, v2); - + axis_angle_to_quat(q, axis, angle); } -void rotation_between_quats_to_quat(float *q, const float q1[4], const float q2[4]) +void rotation_between_quats_to_quat(float q[4], const float q1[4], const float q2[4]) { float tquat[4]; double dot = 0.0f; @@ -405,141 +402,145 @@ void rotation_between_quats_to_quat(float *q, const float q1[4], const float q2[ mul_qt_qtqt(q, tquat, q2); } - void vec_to_quat(float q[4], const float vec[3], short axis, const short upflag) { float q2[4], nor[3], *fp, mat[3][3], angle, si, co, x2, y2, z2, len1; - + assert(axis >= 0 && axis <= 5); assert(upflag >= 0 && upflag <= 2); - + /* first rotate to axis */ - if (axis>2) { - x2= vec[0] ; y2= vec[1] ; z2= vec[2]; - axis-= 3; + if (axis > 2) { + x2 = vec[0]; + y2 = vec[1]; + z2 = vec[2]; + axis -= 3; } else { - x2= -vec[0] ; y2= -vec[1] ; z2= -vec[2]; + x2 = -vec[0]; + y2 = -vec[1]; + z2 = -vec[2]; } - - q[0]=1.0; - q[1]=q[2]=q[3]= 0.0; - len1= (float)sqrt(x2*x2+y2*y2+z2*z2); + q[0] = 1.0; + q[1] = q[2] = q[3] = 0.0; + + len1 = (float)sqrt(x2 * x2 + y2 * y2 + z2 * z2); if (len1 == 0.0f) return; /* nasty! I need a good routine for this... * problem is a rotation of an Y axis to the negative Y-axis for example. */ - if (axis==0) { /* x-axis */ - nor[0]= 0.0; - nor[1]= -z2; - nor[2]= y2; + if (axis == 0) { /* x-axis */ + nor[0] = 0.0; + nor[1] = -z2; + nor[2] = y2; - if (fabs(y2)+fabs(z2)<0.0001) - nor[1]= 1.0; + if (fabs(y2) + fabs(z2) < 0.0001) + nor[1] = 1.0; - co= x2; + co = x2; } - else if (axis==1) { /* y-axis */ - nor[0]= z2; - nor[1]= 0.0; - nor[2]= -x2; - - if (fabs(x2)+fabs(z2)<0.0001) - nor[2]= 1.0; - - co= y2; + else if (axis == 1) { /* y-axis */ + nor[0] = z2; + nor[1] = 0.0; + nor[2] = -x2; + + if (fabs(x2) + fabs(z2) < 0.0001) + nor[2] = 1.0; + + co = y2; } - else { /* z-axis */ - nor[0]= -y2; - nor[1]= x2; - nor[2]= 0.0; + else { /* z-axis */ + nor[0] = -y2; + nor[1] = x2; + nor[2] = 0.0; - if (fabs(x2)+fabs(y2)<0.0001) - nor[0]= 1.0; + if (fabs(x2) + fabs(y2) < 0.0001) + nor[0] = 1.0; - co= z2; + co = z2; } - co/= len1; + co /= len1; normalize_v3(nor); - - angle= 0.5f*saacos(co); - si= (float)sin(angle); - q[0]= (float)cos(angle); - q[1]= nor[0]*si; - q[2]= nor[1]*si; - q[3]= nor[2]*si; - - if (axis!=upflag) { - quat_to_mat3(mat,q); - - fp= mat[2]; - if (axis==0) { - if (upflag==1) angle= (float)(0.5*atan2(fp[2], fp[1])); - else angle= (float)(-0.5*atan2(fp[1], fp[2])); + + angle = 0.5f * saacos(co); + si = (float)sin(angle); + q[0] = (float)cos(angle); + q[1] = nor[0] * si; + q[2] = nor[1] * si; + q[3] = nor[2] * si; + + if (axis != upflag) { + quat_to_mat3(mat, q); + + fp = mat[2]; + if (axis == 0) { + if (upflag == 1) angle = (float)(0.5 * atan2(fp[2], fp[1])); + else angle = (float)(-0.5 * atan2(fp[1], fp[2])); } - else if (axis==1) { - if (upflag==0) angle= (float)(-0.5*atan2(fp[2], fp[0])); - else angle= (float)(0.5*atan2(fp[0], fp[2])); + else if (axis == 1) { + if (upflag == 0) angle = (float)(-0.5 * atan2(fp[2], fp[0])); + else angle = (float)(0.5 * atan2(fp[0], fp[2])); } else { - if (upflag==0) angle= (float)(0.5*atan2(-fp[1], -fp[0])); - else angle= (float)(-0.5*atan2(-fp[0], -fp[1])); + if (upflag == 0) angle = (float)(0.5 * atan2(-fp[1], -fp[0])); + else angle = (float)(-0.5 * atan2(-fp[0], -fp[1])); } - - co= cosf(angle); - si= sinf(angle)/len1; - q2[0]= co; - q2[1]= x2*si; - q2[2]= y2*si; - q2[3]= z2*si; - - mul_qt_qtqt(q,q2,q); + + co = cosf(angle); + si = sinf(angle) / len1; + q2[0] = co; + q2[1] = x2 * si; + q2[2] = y2 * si; + q2[3] = z2 * si; + + mul_qt_qtqt(q, q2, q); } } #if 0 + /* A & M Watt, Advanced animation and rendering techniques, 1992 ACM press */ -void QuatInterpolW(float *result, float *quat1, float *quat2, float t) +void QuatInterpolW(float *result, float quat1[4], float quat2[4], float t) { float omega, cosom, sinom, sc1, sc2; cosom = quat1[0] * quat2[0] + quat1[1] * quat2[1] + quat1[2] * quat2[2] + quat1[3] * quat2[3]; - + /* rotate around shortest angle */ if ((1.0f + cosom) > 0.0001f) { - + if ((1.0f - cosom) > 0.0001f) { omega = (float)acos(cosom); sinom = (float)sin(omega); sc1 = (float)sin((1.0 - t) * omega) / sinom; sc2 = (float)sin(t * omega) / sinom; - } + } else { sc1 = 1.0f - t; sc2 = t; } - result[0] = sc1*quat1[0] + sc2*quat2[0]; - result[1] = sc1*quat1[1] + sc2*quat2[1]; - result[2] = sc1*quat1[2] + sc2*quat2[2]; - result[3] = sc1*quat1[3] + sc2*quat2[3]; - } + result[0] = sc1 * quat1[0] + sc2 * quat2[0]; + result[1] = sc1 * quat1[1] + sc2 * quat2[1]; + result[2] = sc1 * quat1[2] + sc2 * quat2[2]; + result[3] = sc1 * quat1[3] + sc2 * quat2[3]; + } else { result[0] = quat2[3]; result[1] = -quat2[2]; result[2] = quat2[1]; result[3] = -quat2[0]; - - sc1 = (float)sin((1.0 - t)*M_PI_2); - sc2 = (float)sin(t*M_PI_2); - - result[0] = sc1*quat1[0] + sc2*result[0]; - result[1] = sc1*quat1[1] + sc2*result[1]; - result[2] = sc1*quat1[2] + sc2*result[2]; - result[3] = sc1*quat1[3] + sc2*result[3]; + + sc1 = (float)sin((1.0 - t) * M_PI_2); + sc2 = (float)sin(t * M_PI_2); + + result[0] = sc1 * quat1[0] + sc2 * result[0]; + result[1] = sc1 * quat1[1] + sc2 * result[1]; + result[2] = sc1 * quat1[2] + sc2 * result[2]; + result[3] = sc1 * quat1[3] + sc2 * result[3]; } } #endif @@ -553,18 +554,18 @@ void interp_qt_qtqt(float result[4], const float quat1[4], const float quat2[4], /* rotate around shortest angle */ if (cosom < 0.0f) { cosom = -cosom; - quat[0]= -quat1[0]; - quat[1]= -quat1[1]; - quat[2]= -quat1[2]; - quat[3]= -quat1[3]; - } + quat[0] = -quat1[0]; + quat[1] = -quat1[1]; + quat[2] = -quat1[2]; + quat[3] = -quat1[3]; + } else { - quat[0]= quat1[0]; - quat[1]= quat1[1]; - quat[2]= quat1[2]; - quat[3]= quat1[3]; + quat[0] = quat1[0]; + quat[1] = quat1[1]; + quat[2] = quat1[2]; + quat[3] = quat1[3]; } - + if ((1.0f - cosom) > 0.0001f) { omega = (float)acos(cosom); sinom = (float)sin(omega); @@ -572,10 +573,10 @@ void interp_qt_qtqt(float result[4], const float quat1[4], const float quat2[4], sc2 = (float)sin(t * omega) / sinom; } else { - sc1= 1.0f - t; - sc2= t; + sc1 = 1.0f - t; + sc2 = t; } - + result[0] = sc1 * quat[0] + sc2 * quat2[0]; result[1] = sc1 * quat[1] + sc2 * quat2[1]; result[2] = sc1 * quat[2] + sc2 * quat2[2]; @@ -584,57 +585,57 @@ void interp_qt_qtqt(float result[4], const float quat1[4], const float quat2[4], void add_qt_qtqt(float result[4], const float quat1[4], const float quat2[4], const float t) { - result[0]= quat1[0] + t*quat2[0]; - result[1]= quat1[1] + t*quat2[1]; - result[2]= quat1[2] + t*quat2[2]; - result[3]= quat1[3] + t*quat2[3]; + result[0] = quat1[0] + t * quat2[0]; + result[1] = quat1[1] + t * quat2[1]; + result[2] = quat1[2] + t * quat2[2]; + result[3] = quat1[3] + t * quat2[3]; } void tri_to_quat(float quat[4], const float v1[3], const float v2[3], const float v3[3]) { /* imaginary x-axis, y-axis triangle is being rotated */ float vec[3], q1[4], q2[4], n[3], si, co, angle, mat[3][3], imat[3][3]; - + /* move z-axis to face-normal */ - normal_tri_v3(vec,v1, v2, v3); + normal_tri_v3(vec, v1, v2, v3); - n[0]= vec[1]; - n[1]= -vec[0]; - n[2]= 0.0f; + n[0] = vec[1]; + n[1] = -vec[0]; + n[2] = 0.0f; normalize_v3(n); - - if (n[0]==0.0f && n[1]==0.0f) n[0]= 1.0f; - - angle= -0.5f*(float)saacos(vec[2]); - co= (float)cos(angle); - si= (float)sin(angle); - q1[0]= co; - q1[1]= n[0]*si; - q1[2]= n[1]*si; - q1[3]= 0.0f; - + + if (n[0] == 0.0f && n[1] == 0.0f) n[0] = 1.0f; + + angle = -0.5f * (float)saacos(vec[2]); + co = (float)cos(angle); + si = (float)sin(angle); + q1[0] = co; + q1[1] = n[0] * si; + q1[2] = n[1] * si; + q1[3] = 0.0f; + /* rotate back line v1-v2 */ - quat_to_mat3(mat,q1); + quat_to_mat3(mat, q1); invert_m3_m3(imat, mat); sub_v3_v3v3(vec, v2, v1); mul_m3_v3(imat, vec); /* what angle has this line with x-axis? */ - vec[2]= 0.0f; + vec[2] = 0.0f; normalize_v3(vec); - angle= (float)(0.5*atan2(vec[1], vec[0])); - co= (float)cos(angle); - si= (float)sin(angle); - q2[0]= co; - q2[1]= 0.0f; - q2[2]= 0.0f; - q2[3]= si; + angle = (float)(0.5 * atan2(vec[1], vec[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(quat, q1, q2); } -void print_qt(const char *str, const float q[4]) +void print_qt(const char *str, const float q[4]) { printf("%s: %.3f %.3f %.3f %.3f\n", str, q[0], q[1], q[2], q[3]); } @@ -657,7 +658,7 @@ void axis_angle_to_quat(float q[4], const float axis[3], float angle) q[0] = (float)cos(angle); q[1] = nor[0] * si; q[2] = nor[1] * si; - q[3] = nor[2] * si; + q[3] = nor[2] * si; } /* Quaternions to Axis Angle */ @@ -666,67 +667,67 @@ void quat_to_axis_angle(float axis[3], float *angle, const float q[4]) float ha, si; #ifdef DEBUG - if (!((ha=dot_qtqt(q, q))==0.0f || (fabsf(ha-1.0f) < (float)QUAT_EPSILON))) { + if (!((ha = dot_qtqt(q, q)) == 0.0f || (fabsf(ha - 1.0f) < (float)QUAT_EPSILON))) { fprintf(stderr, "Warning! quat_to_axis_angle() called with non-normalized: size %.8f *** report a bug ***\n", ha); } #endif /* calculate angle/2, and sin(angle/2) */ - ha= (float)acos(q[0]); - si= (float)sin(ha); - + ha = (float)acos(q[0]); + si = (float)sin(ha); + /* from half-angle to angle */ - *angle= ha * 2; - + *angle = ha * 2; + /* prevent division by zero for axis conversion */ if (fabs(si) < 0.0005) - si= 1.0f; - - axis[0]= q[1] / si; - axis[1]= q[2] / si; - axis[2]= q[3] / si; + si = 1.0f; + + axis[0] = q[1] / si; + axis[1] = q[2] / si; + axis[2] = q[3] / si; } /* Axis Angle to Euler Rotation */ void axis_angle_to_eulO(float eul[3], const short order, const float axis[3], const float angle) { float q[4]; - + /* use quaternions as intermediate representation for now... */ axis_angle_to_quat(q, axis, angle); - quat_to_eulO(eul, order,q); + quat_to_eulO(eul, order, q); } /* Euler Rotation to Axis Angle */ void eulO_to_axis_angle(float axis[3], float *angle, const float eul[3], const short order) { float q[4]; - + /* use quaternions as intermediate representation for now... */ - eulO_to_quat(q,eul, order); - quat_to_axis_angle(axis, angle,q); + eulO_to_quat(q, eul, order); + quat_to_axis_angle(axis, angle, q); } /* axis angle to 3x3 matrix - safer version (normalization of axis performed) */ void axis_angle_to_mat3(float mat[3][3], const float axis[3], const float angle) { float nor[3], nsi[3], co, si, ico; - + /* normalize the axis first (to remove unwanted scaling) */ if (normalize_v3_v3(nor, axis) == 0.0f) { unit_m3(mat); return; } - + /* now convert this to a 3x3 matrix */ - co= (float)cos(angle); - si= (float)sin(angle); - - ico= (1.0f - co); - nsi[0]= nor[0]*si; - nsi[1]= nor[1]*si; - nsi[2]= nor[2]*si; - + co = (float)cos(angle); + si = (float)sin(angle); + + ico = (1.0f - co); + nsi[0] = nor[0] * si; + nsi[1] = nor[1] * si; + nsi[2] = nor[2] * si; + mat[0][0] = ((nor[0] * nor[0]) * ico) + co; mat[0][1] = ((nor[0] * nor[1]) * ico) + nsi[2]; mat[0][2] = ((nor[0] * nor[2]) * ico) - nsi[1]; @@ -742,77 +743,75 @@ void axis_angle_to_mat3(float mat[3][3], const float axis[3], const float angle) void axis_angle_to_mat4(float mat[4][4], const float axis[3], const float angle) { float tmat[3][3]; - - axis_angle_to_mat3(tmat,axis, angle); + + axis_angle_to_mat3(tmat, axis, angle); unit_m4(mat); copy_m4_m3(mat, tmat); } /* 3x3 matrix to axis angle (see Mat4ToVecRot too) */ -void mat3_to_axis_angle(float axis[3], float *angle,float mat[3][3]) +void mat3_to_axis_angle(float axis[3], float *angle, float mat[3][3]) { float q[4]; - + /* use quaternions as intermediate representation */ // TODO: it would be nicer to go straight there... - mat3_to_quat(q,mat); - quat_to_axis_angle(axis, angle,q); + mat3_to_quat(q, mat); + quat_to_axis_angle(axis, angle, q); } /* 4x4 matrix to axis angle (see Mat4ToVecRot too) */ -void mat4_to_axis_angle(float axis[3], float *angle,float mat[4][4]) +void mat4_to_axis_angle(float axis[3], float *angle, float mat[4][4]) { float q[4]; - + /* use quaternions as intermediate representation */ // TODO: it would be nicer to go straight there... - mat4_to_quat(q,mat); - quat_to_axis_angle(axis, angle,q); + mat4_to_quat(q, mat); + quat_to_axis_angle(axis, angle, q); } - - void single_axis_angle_to_mat3(float mat[3][3], const char axis, const float angle) { - const float angle_cos= cosf(angle); - const float angle_sin= sinf(angle); - - switch(axis) { - case 'X': /* rotation around X */ - mat[0][0] = 1.0f; - mat[0][1] = 0.0f; - mat[0][2] = 0.0f; - mat[1][0] = 0.0f; - mat[1][1] = angle_cos; - mat[1][2] = angle_sin; - mat[2][0] = 0.0f; - mat[2][1] = -angle_sin; - mat[2][2] = angle_cos; - break; - case 'Y': /* rotation around Y */ - mat[0][0] = angle_cos; - mat[0][1] = 0.0f; - mat[0][2] = -angle_sin; - mat[1][0] = 0.0f; - mat[1][1] = 1.0f; - mat[1][2] = 0.0f; - mat[2][0] = angle_sin; - mat[2][1] = 0.0f; - mat[2][2] = angle_cos; - break; - case 'Z': /* rotation around Z */ - mat[0][0] = angle_cos; - mat[0][1] = angle_sin; - mat[0][2] = 0.0f; - mat[1][0] = -angle_sin; - mat[1][1] = angle_cos; - mat[1][2] = 0.0f; - mat[2][0] = 0.0f; - mat[2][1] = 0.0f; - mat[2][2] = 1.0f; - break; - default: - assert(0); + const float angle_cos = cosf(angle); + const float angle_sin = sinf(angle); + + switch (axis) { + case 'X': /* rotation around X */ + mat[0][0] = 1.0f; + mat[0][1] = 0.0f; + mat[0][2] = 0.0f; + mat[1][0] = 0.0f; + mat[1][1] = angle_cos; + mat[1][2] = angle_sin; + mat[2][0] = 0.0f; + mat[2][1] = -angle_sin; + mat[2][2] = angle_cos; + break; + case 'Y': /* rotation around Y */ + mat[0][0] = angle_cos; + mat[0][1] = 0.0f; + mat[0][2] = -angle_sin; + mat[1][0] = 0.0f; + mat[1][1] = 1.0f; + mat[1][2] = 0.0f; + mat[2][0] = angle_sin; + mat[2][1] = 0.0f; + mat[2][2] = angle_cos; + break; + case 'Z': /* rotation around Z */ + mat[0][0] = angle_cos; + mat[0][1] = angle_sin; + mat[0][2] = 0.0f; + mat[1][0] = -angle_sin; + mat[1][1] = angle_cos; + mat[1][2] = 0.0f; + mat[2][0] = 0.0f; + mat[2][1] = 0.0f; + mat[2][2] = 1.0f; + break; + default: + assert(0); } } @@ -824,33 +823,33 @@ void vec_rot_to_mat3(float mat[][3], const float vec[3], const float phi) { /* rotation of phi radials around vec */ float vx, vx2, vy, vy2, vz, vz2, co, si; - - vx= vec[0]; - vy= vec[1]; - vz= vec[2]; - vx2= vx*vx; - vy2= vy*vy; - vz2= vz*vz; - co= (float)cos(phi); - si= (float)sin(phi); - - mat[0][0]= vx2+co*(1.0f-vx2); - mat[0][1]= vx*vy*(1.0f-co)+vz*si; - mat[0][2]= vz*vx*(1.0f-co)-vy*si; - mat[1][0]= vx*vy*(1.0f-co)-vz*si; - mat[1][1]= vy2+co*(1.0f-vy2); - mat[1][2]= vy*vz*(1.0f-co)+vx*si; - mat[2][0]= vz*vx*(1.0f-co)+vy*si; - mat[2][1]= vy*vz*(1.0f-co)-vx*si; - mat[2][2]= vz2+co*(1.0f-vz2); + + vx = vec[0]; + vy = vec[1]; + vz = vec[2]; + vx2 = vx * vx; + vy2 = vy * vy; + vz2 = vz * vz; + co = (float)cos(phi); + si = (float)sin(phi); + + mat[0][0] = vx2 + co * (1.0f - vx2); + mat[0][1] = vx * vy * (1.0f - co) + vz * si; + mat[0][2] = vz * vx * (1.0f - co) - vy * si; + mat[1][0] = vx * vy * (1.0f - co) - vz * si; + mat[1][1] = vy2 + co * (1.0f - vy2); + mat[1][2] = vy * vz * (1.0f - co) + vx * si; + mat[2][0] = vz * vx * (1.0f - co) + vy * si; + mat[2][1] = vy * vz * (1.0f - co) - vx * si; + mat[2][2] = vz2 + co * (1.0f - vz2); } /* axis angle to 4x4 matrix */ void vec_rot_to_mat4(float mat[][4], const float vec[3], const float phi) { float tmat[3][3]; - - vec_rot_to_mat3(tmat,vec, phi); + + vec_rot_to_mat3(tmat, vec, phi); unit_m4(mat); copy_m4_m3(mat, tmat); } @@ -861,16 +860,16 @@ void vec_rot_to_quat(float *quat, const float vec[3], const float phi) /* rotation of phi radials around vec */ float si; - quat[1]= vec[0]; - quat[2]= vec[1]; - quat[3]= vec[2]; - - if (normalize_v3(quat+1) == 0.0f) { + quat[1] = vec[0]; + quat[2] = vec[1]; + quat[3] = vec[2]; + + if (normalize_v3(quat + 1) == 0.0f) { unit_qt(quat); } else { - quat[0]= (float)cos((double)phi/2.0); - si= (float)sin((double)phi/2.0); + quat[0] = (float)cos((double)phi / 2.0); + si = (float)sin((double)phi / 2.0); quat[1] *= si; quat[2] *= si; quat[3] *= si; @@ -883,27 +882,27 @@ void vec_rot_to_quat(float *quat, const float vec[3], const float phi) void eul_to_mat3(float mat[][3], const float eul[3]) { double ci, cj, ch, si, sj, sh, cc, cs, sc, ss; - - ci = cos(eul[0]); - cj = cos(eul[1]); + + ci = cos(eul[0]); + cj = cos(eul[1]); ch = cos(eul[2]); - si = sin(eul[0]); - sj = sin(eul[1]); + si = sin(eul[0]); + sj = sin(eul[1]); sh = sin(eul[2]); - cc = ci*ch; - cs = ci*sh; - sc = si*ch; - ss = si*sh; + cc = ci * ch; + cs = ci * sh; + sc = si * ch; + ss = si * sh; - mat[0][0] = (float)(cj*ch); - mat[1][0] = (float)(sj*sc-cs); - mat[2][0] = (float)(sj*cc+ss); - mat[0][1] = (float)(cj*sh); - mat[1][1] = (float)(sj*ss+cc); - mat[2][1] = (float)(sj*cs-sc); - mat[0][2] = (float)-sj; - mat[1][2] = (float)(cj*si); - mat[2][2] = (float)(cj*ci); + mat[0][0] = (float)(cj * ch); + mat[1][0] = (float)(sj * sc - cs); + mat[2][0] = (float)(sj * cc + ss); + mat[0][1] = (float)(cj * sh); + mat[1][1] = (float)(sj * ss + cc); + mat[2][1] = (float)(sj * cs - sc); + mat[0][2] = (float)-sj; + mat[1][2] = (float)(cj * si); + mat[2][2] = (float)(cj * ci); } @@ -911,75 +910,76 @@ void eul_to_mat3(float mat[][3], const float eul[3]) void eul_to_mat4(float mat[][4], const float eul[3]) { double ci, cj, ch, si, sj, sh, cc, cs, sc, ss; - - ci = cos(eul[0]); - cj = cos(eul[1]); + + ci = cos(eul[0]); + cj = cos(eul[1]); ch = cos(eul[2]); - si = sin(eul[0]); - sj = sin(eul[1]); + si = sin(eul[0]); + sj = sin(eul[1]); sh = sin(eul[2]); - cc = ci*ch; - cs = ci*sh; - sc = si*ch; - ss = si*sh; + cc = ci * ch; + cs = ci * sh; + sc = si * ch; + ss = si * sh; - mat[0][0] = (float)(cj*ch); - mat[1][0] = (float)(sj*sc-cs); - mat[2][0] = (float)(sj*cc+ss); - mat[0][1] = (float)(cj*sh); - mat[1][1] = (float)(sj*ss+cc); - mat[2][1] = (float)(sj*cs-sc); - mat[0][2] = (float)-sj; - mat[1][2] = (float)(cj*si); - mat[2][2] = (float)(cj*ci); + mat[0][0] = (float)(cj * ch); + mat[1][0] = (float)(sj * sc - cs); + mat[2][0] = (float)(sj * cc + ss); + mat[0][1] = (float)(cj * sh); + mat[1][1] = (float)(sj * ss + cc); + mat[2][1] = (float)(sj * cs - sc); + mat[0][2] = (float)-sj; + mat[1][2] = (float)(cj * si); + mat[2][2] = (float)(cj * ci); - mat[3][0]= mat[3][1]= mat[3][2]= mat[0][3]= mat[1][3]= mat[2][3]= 0.0f; - mat[3][3]= 1.0f; + mat[3][0] = mat[3][1] = mat[3][2] = mat[0][3] = mat[1][3] = mat[2][3] = 0.0f; + mat[3][3] = 1.0f; } /* returns two euler calculation methods, so we can pick the best */ + /* XYZ order */ static void mat3_to_eul2(float tmat[][3], float eul1[3], float eul2[3]) { float cy, quat[4], mat[3][3]; - - mat3_to_quat(quat,tmat); - quat_to_mat3(mat,quat); + + mat3_to_quat(quat, tmat); + quat_to_mat3(mat, quat); copy_m3_m3(mat, tmat); normalize_m3(mat); - - cy = (float)sqrt(mat[0][0]*mat[0][0] + mat[0][1]*mat[0][1]); - - if (cy > 16.0f*FLT_EPSILON) { - + + cy = (float)sqrt(mat[0][0] * mat[0][0] + mat[0][1] * mat[0][1]); + + if (cy > 16.0f * FLT_EPSILON) { + eul1[0] = (float)atan2(mat[1][2], mat[2][2]); eul1[1] = (float)atan2(-mat[0][2], cy); eul1[2] = (float)atan2(mat[0][1], mat[0][0]); - + eul2[0] = (float)atan2(-mat[1][2], -mat[2][2]); eul2[1] = (float)atan2(-mat[0][2], -cy); eul2[2] = (float)atan2(-mat[0][1], -mat[0][0]); - + } else { eul1[0] = (float)atan2(-mat[2][1], mat[1][1]); eul1[1] = (float)atan2(-mat[0][2], cy); eul1[2] = 0.0f; - + copy_v3_v3(eul2, eul1); } } /* XYZ order */ -void mat3_to_eul(float *eul,float tmat[][3]) +void mat3_to_eul(float *eul, float tmat[][3]) { float eul1[3], eul2[3]; - + mat3_to_eul2(tmat, eul1, eul2); - + /* return best, which is just the one with lowest values it in */ - if (fabs(eul1[0])+fabs(eul1[1])+fabs(eul1[2]) > fabs(eul2[0])+fabs(eul2[1])+fabs(eul2[2])) { + if (fabs(eul1[0]) + fabs(eul1[1]) + fabs(eul1[2]) > fabs(eul2[0]) + fabs(eul2[1]) + fabs(eul2[2])) { copy_v3_v3(eul, eul2); } else { @@ -988,13 +988,13 @@ void mat3_to_eul(float *eul,float tmat[][3]) } /* XYZ order */ -void mat4_to_eul(float *eul,float tmat[][4]) +void mat4_to_eul(float *eul, float tmat[][4]) { float tempMat[3][3]; copy_m3_m4(tempMat, tmat); normalize_m3(tempMat); - mat3_to_eul(eul,tempMat); + mat3_to_eul(eul, tempMat); } /* XYZ order */ @@ -1002,24 +1002,33 @@ void quat_to_eul(float *eul, const float quat[4]) { float mat[3][3]; - quat_to_mat3(mat,quat); - mat3_to_eul(eul,mat); + quat_to_mat3(mat, quat); + mat3_to_eul(eul, mat); } /* XYZ order */ void eul_to_quat(float *quat, const float eul[3]) { float ti, tj, th, ci, cj, ch, si, sj, sh, cc, cs, sc, ss; - - ti = eul[0]*0.5f; tj = eul[1]*0.5f; th = eul[2]*0.5f; - ci = (float)cos(ti); cj = (float)cos(tj); ch = (float)cos(th); - si = (float)sin(ti); sj = (float)sin(tj); sh = (float)sin(th); - cc = ci*ch; cs = ci*sh; sc = si*ch; ss = si*sh; - - quat[0] = cj*cc + sj*ss; - quat[1] = cj*sc - sj*cs; - quat[2] = cj*ss + sj*cc; - quat[3] = cj*cs - sj*sc; + + ti = eul[0] * 0.5f; + tj = eul[1] * 0.5f; + th = eul[2] * 0.5f; + ci = cosf(ti); + cj = cosf(tj); + ch = cosf(th); + si = sinf(ti); + sj = sinf(tj); + sh = sinf(th); + cc = ci * ch; + cs = ci * sh; + sc = si * ch; + ss = si * sh; + + quat[0] = cj * cc + sj * ss; + quat[1] = cj * sc - sj * cs; + quat[2] = cj * ss + sj * cc; + quat[3] = cj * cs - sj * sc; } /* XYZ order */ @@ -1029,99 +1038,116 @@ void rotate_eul(float *beul, const char axis, const float ang) assert(axis >= 'X' && axis <= 'Z'); - eul[0]= eul[1]= eul[2]= 0.0f; - if (axis=='X') eul[0]= ang; - else if (axis=='Y') eul[1]= ang; - else eul[2]= ang; - - eul_to_mat3(mat1,eul); - eul_to_mat3(mat2,beul); - + eul[0] = eul[1] = eul[2] = 0.0f; + if (axis == 'X') eul[0] = ang; + else if (axis == 'Y') eul[1] = ang; + else eul[2] = ang; + + eul_to_mat3(mat1, eul); + eul_to_mat3(mat2, beul); + mul_m3_m3m3(totmat, mat2, mat1); - - mat3_to_eul(beul,totmat); - + + mat3_to_eul(beul, totmat); + } /* exported to transform.c */ + /* order independent! */ void compatible_eul(float eul[3], const float oldrot[3]) { float dx, dy, dz; - + /* correct differences of about 360 degrees first */ - dx= eul[0] - oldrot[0]; - dy= eul[1] - oldrot[1]; - dz= eul[2] - oldrot[2]; - + dx = eul[0] - oldrot[0]; + dy = eul[1] - oldrot[1]; + dz = eul[2] - oldrot[2]; + while (fabs(dx) > 5.1) { - if (dx > 0.0f) eul[0] -= 2.0f*(float)M_PI; else eul[0]+= 2.0f*(float)M_PI; - dx= eul[0] - oldrot[0]; + if (dx > 0.0f) eul[0] -= 2.0f * (float)M_PI; + else eul[0] += 2.0f * (float)M_PI; + dx = eul[0] - oldrot[0]; } while (fabs(dy) > 5.1) { - if (dy > 0.0f) eul[1] -= 2.0f*(float)M_PI; else eul[1]+= 2.0f*(float)M_PI; - dy= eul[1] - oldrot[1]; + if (dy > 0.0f) eul[1] -= 2.0f * (float)M_PI; + else eul[1] += 2.0f * (float)M_PI; + dy = eul[1] - oldrot[1]; } while (fabs(dz) > 5.1) { - if (dz > 0.0f) eul[2] -= 2.0f*(float)M_PI; else eul[2]+= 2.0f*(float)M_PI; - dz= eul[2] - oldrot[2]; + if (dz > 0.0f) eul[2] -= 2.0f * (float)M_PI; + else eul[2] += 2.0f * (float)M_PI; + dz = eul[2] - oldrot[2]; } - - /* is 1 of the axis rotations larger than 180 degrees and the other small? NO ELSE IF!! */ - if (fabs(dx) > 3.2 && fabs(dy)<1.6 && fabs(dz)<1.6) { - if (dx > 0.0f) eul[0] -= 2.0f*(float)M_PI; else eul[0]+= 2.0f*(float)M_PI; + + /* is 1 of the axis rotations larger than 180 degrees and the other small? NO ELSE IF!! */ + if (fabs(dx) > 3.2 && fabs(dy) < 1.6 && fabs(dz) < 1.6) { + if (dx > 0.0f) eul[0] -= 2.0f * (float)M_PI; + else eul[0] += 2.0f * (float)M_PI; } - if (fabs(dy) > 3.2 && fabs(dz)<1.6 && fabs(dx)<1.6) { - if (dy > 0.0f) eul[1] -= 2.0f*(float)M_PI; else eul[1]+= 2.0f*(float)M_PI; + if (fabs(dy) > 3.2 && fabs(dz) < 1.6 && fabs(dx) < 1.6) { + if (dy > 0.0f) eul[1] -= 2.0f * (float)M_PI; + else eul[1] += 2.0f * (float)M_PI; } - if (fabs(dz) > 3.2 && fabs(dx)<1.6 && fabs(dy)<1.6) { - if (dz > 0.0f) eul[2] -= 2.0f*(float)M_PI; else eul[2]+= 2.0f*(float)M_PI; + if (fabs(dz) > 3.2 && fabs(dx) < 1.6 && fabs(dy) < 1.6) { + if (dz > 0.0f) eul[2] -= 2.0f * (float)M_PI; + else eul[2] += 2.0f * (float)M_PI; } - + /* the method below was there from ancient days... but why! probably because the code sucks :) */ -#if 0 +#if 0 /* calc again */ - dx= eul[0] - oldrot[0]; - dy= eul[1] - oldrot[1]; - dz= eul[2] - oldrot[2]; - + dx = eul[0] - oldrot[0]; + dy = eul[1] - oldrot[1]; + dz = eul[2] - oldrot[2]; + /* special case, tested for x-z */ - + if ((fabs(dx) > 3.1 && fabs(dz) > 1.5) || (fabs(dx) > 1.5 && fabs(dz) > 3.1)) { - if (dx > 0.0) eul[0] -= M_PI; else eul[0]+= M_PI; - if (eul[1] > 0.0) eul[1]= M_PI - eul[1]; else eul[1]= -M_PI - eul[1]; - if (dz > 0.0) eul[2] -= M_PI; else eul[2]+= M_PI; - + if (dx > 0.0) eul[0] -= M_PI; + else eul[0] += M_PI; + if (eul[1] > 0.0) eul[1] = M_PI - eul[1]; + else eul[1] = -M_PI - eul[1]; + if (dz > 0.0) eul[2] -= M_PI; + else eul[2] += M_PI; + } else if ((fabs(dx) > 3.1 && fabs(dy) > 1.5) || (fabs(dx) > 1.5 && fabs(dy) > 3.1)) { - if (dx > 0.0) eul[0] -= M_PI; else eul[0]+= M_PI; - if (dy > 0.0) eul[1] -= M_PI; else eul[1]+= M_PI; - if (eul[2] > 0.0) eul[2]= M_PI - eul[2]; else eul[2]= -M_PI - eul[2]; + if (dx > 0.0) eul[0] -= M_PI; + else eul[0] += M_PI; + if (dy > 0.0) eul[1] -= M_PI; + else eul[1] += M_PI; + if (eul[2] > 0.0) eul[2] = M_PI - eul[2]; + else eul[2] = -M_PI - eul[2]; } else if ((fabs(dy) > 3.1 && fabs(dz) > 1.5) || (fabs(dy) > 1.5 && fabs(dz) > 3.1)) { - if (eul[0] > 0.0) eul[0]= M_PI - eul[0]; else eul[0]= -M_PI - eul[0]; - if (dy > 0.0) eul[1] -= M_PI; else eul[1]+= M_PI; - if (dz > 0.0) eul[2] -= M_PI; else eul[2]+= M_PI; + if (eul[0] > 0.0) eul[0] = M_PI - eul[0]; + else eul[0] = -M_PI - eul[0]; + if (dy > 0.0) eul[1] -= M_PI; + else eul[1] += M_PI; + if (dz > 0.0) eul[2] -= M_PI; + else eul[2] += M_PI; } -#endif +#endif } /* uses 2 methods to retrieve eulers, and picks the closest */ + /* XYZ order */ void mat3_to_compatible_eul(float eul[3], const float oldrot[3], float mat[][3]) { float eul1[3], eul2[3]; float d1, d2; - + mat3_to_eul2(mat, eul1, eul2); - + compatible_eul(eul1, oldrot); compatible_eul(eul2, oldrot); - - d1= (float)fabs(eul1[0]-oldrot[0]) + (float)fabs(eul1[1]-oldrot[1]) + (float)fabs(eul1[2]-oldrot[2]); - d2= (float)fabs(eul2[0]-oldrot[0]) + (float)fabs(eul2[1]-oldrot[1]) + (float)fabs(eul2[2]-oldrot[2]); - + + d1 = (float)fabs(eul1[0] - oldrot[0]) + (float)fabs(eul1[1] - oldrot[1]) + (float)fabs(eul1[2] - oldrot[2]); + d2 = (float)fabs(eul2[0] - oldrot[0]) + (float)fabs(eul2[1] - oldrot[1]) + (float)fabs(eul2[2] - oldrot[2]); + /* return best, which is just the one with lowest difference */ if (d1 > d2) { copy_v3_v3(eul, eul2); @@ -1129,13 +1155,13 @@ void mat3_to_compatible_eul(float eul[3], const float oldrot[3], float mat[][3]) else { copy_v3_v3(eul, eul1); } - + } /************************** Arbitrary Order Eulers ***************************/ /* Euler Rotation Order Code: - * was adapted from + * was adapted from * ANSI C code from the article * "Euler Angle Conversion" * by Ken Shoemake, shoemake@graphics.cis.upenn.edu @@ -1146,10 +1172,10 @@ void mat3_to_compatible_eul(float eul[3], const float oldrot[3], float mat[][3]) /* Type for rotation order info - see wiki for derivation details */ typedef struct RotOrderInfo { short axis[3]; - short parity; /* parity of axis permutation (even=0, odd=1) - 'n' in original code */ + short parity; /* parity of axis permutation (even=0, odd=1) - 'n' in original code */ } RotOrderInfo; -/* Array of info for Rotation Order calculations +/* Array of info for Rotation Order calculations * WARNING: must be kept in same order as eEulerRotationOrders */ static RotOrderInfo rotOrders[]= { @@ -1162,8 +1188,8 @@ static RotOrderInfo rotOrders[]= { {{2, 1, 0}, 1} // ZYX }; -/* Get relevant pointer to rotation order set from the array - * NOTE: since we start at 1 for the values, but arrays index from 0, +/* Get relevant pointer to rotation order set from the array + * NOTE: since we start at 1 for the values, but arrays index from 0, * there is -1 factor involved in this process... */ #define GET_ROTATIONORDER_INFO(order) (assert(order>=0 && order<=6), (order < 1) ? &rotOrders[0] : &rotOrders[(order)-1]) @@ -1171,105 +1197,127 @@ static RotOrderInfo rotOrders[]= { /* Construct quaternion from Euler angles (in radians). */ void eulO_to_quat(float q[4], const float e[3], const short order) { - RotOrderInfo *R= GET_ROTATIONORDER_INFO(order); - short i=R->axis[0], j=R->axis[1], k=R->axis[2]; + RotOrderInfo *R = GET_ROTATIONORDER_INFO(order); + short i = R->axis[0], j = R->axis[1], k = R->axis[2]; double ti, tj, th, ci, cj, ch, si, sj, sh, cc, cs, sc, ss; double a[3]; - + ti = e[i] * 0.5f; tj = e[j] * (R->parity ? -0.5f : 0.5f); th = e[k] * 0.5f; - ci = cos(ti); cj = cos(tj); ch = cos(th); - si = sin(ti); sj = sin(tj); sh = sin(th); - - cc = ci*ch; cs = ci*sh; - sc = si*ch; ss = si*sh; - - a[i] = cj*sc - sj*cs; - a[j] = cj*ss + sj*cc; - a[k] = cj*cs - sj*sc; - - q[0] = cj*cc + sj*ss; + ci = cos(ti); + cj = cos(tj); + ch = cos(th); + si = sin(ti); + sj = sin(tj); + sh = sin(th); + + cc = ci * ch; + cs = ci * sh; + sc = si * ch; + ss = si * sh; + + a[i] = cj * sc - sj * cs; + a[j] = cj * ss + sj * cc; + a[k] = cj * cs - sj * sc; + + q[0] = cj * cc + sj * ss; q[1] = a[0]; q[2] = a[1]; q[3] = a[2]; - - if (R->parity) q[j+1] = -q[j+1]; + + if (R->parity) q[j + 1] = -q[j + 1]; } /* Convert quaternion to Euler angles (in radians). */ void quat_to_eulO(float e[3], short const order, const float q[4]) { float M[3][3]; - - quat_to_mat3(M,q); - mat3_to_eulO(e, order,M); + + quat_to_mat3(M, q); + mat3_to_eulO(e, order, M); } /* Construct 3x3 matrix from Euler angles (in radians). */ void eulO_to_mat3(float M[3][3], const float e[3], const short order) { - RotOrderInfo *R= GET_ROTATIONORDER_INFO(order); - short i=R->axis[0], j=R->axis[1], k=R->axis[2]; + RotOrderInfo *R = GET_ROTATIONORDER_INFO(order); + short i = R->axis[0], j = R->axis[1], k = R->axis[2]; double ti, tj, th, ci, cj, ch, si, sj, sh, cc, cs, sc, ss; - + if (R->parity) { - ti = -e[i]; tj = -e[j]; th = -e[k]; + ti = -e[i]; + tj = -e[j]; + th = -e[k]; } else { - ti = e[i]; tj = e[j]; th = e[k]; + ti = e[i]; + tj = e[j]; + th = e[k]; } - - ci = cos(ti); cj = cos(tj); ch = cos(th); - si = sin(ti); sj = sin(tj); sh = sin(th); - - cc = ci*ch; cs = ci*sh; - sc = si*ch; ss = si*sh; - - M[i][i] = cj*ch; M[j][i] = sj*sc-cs; M[k][i] = sj*cc+ss; - M[i][j] = cj*sh; M[j][j] = sj*ss+cc; M[k][j] = sj*cs-sc; - M[i][k] = -sj; M[j][k] = cj*si; M[k][k] = cj*ci; + + ci = cos(ti); + cj = cos(tj); + ch = cos(th); + si = sin(ti); + sj = sin(tj); + sh = sin(th); + + cc = ci * ch; + cs = ci * sh; + sc = si * ch; + ss = si * sh; + + M[i][i] = cj * ch; + M[j][i] = sj * sc - cs; + M[k][i] = sj * cc + ss; + M[i][j] = cj * sh; + M[j][j] = sj * ss + cc; + M[k][j] = sj * cs - sc; + M[i][k] = -sj; + M[j][k] = cj * si; + M[k][k] = cj * ci; } /* returns two euler calculation methods, so we can pick the best */ static void mat3_to_eulo2(float M[3][3], float *e1, float *e2, short order) { - RotOrderInfo *R= GET_ROTATIONORDER_INFO(order); - short i=R->axis[0], j=R->axis[1], k=R->axis[2]; + RotOrderInfo *R = GET_ROTATIONORDER_INFO(order); + short i = R->axis[0], j = R->axis[1], k = R->axis[2]; float m[3][3]; double cy; - + /* process the matrix first */ copy_m3_m3(m, M); normalize_m3(m); - - cy= sqrt(m[i][i]*m[i][i] + m[i][j]*m[i][j]); - - if (cy > 16.0*(double)FLT_EPSILON) { + + cy = sqrt(m[i][i] * m[i][i] + m[i][j] * m[i][j]); + + if (cy > 16.0 * (double)FLT_EPSILON) { e1[i] = atan2(m[j][k], m[k][k]); e1[j] = atan2(-m[i][k], cy); e1[k] = atan2(m[i][j], m[i][i]); - + e2[i] = atan2(-m[j][k], -m[k][k]); e2[j] = atan2(-m[i][k], -cy); e2[k] = atan2(-m[i][j], -m[i][i]); - } + } else { e1[i] = atan2(-m[k][j], m[j][j]); e1[j] = atan2(-m[i][k], cy); e1[k] = 0; - + copy_v3_v3(e2, e1); } - + if (R->parity) { - e1[0] = -e1[0]; - e1[1] = -e1[1]; + e1[0] = -e1[0]; + e1[1] = -e1[1]; e1[2] = -e1[2]; - - e2[0] = -e2[0]; - e2[1] = -e2[1]; + + e2[0] = -e2[0]; + e2[1] = -e2[1]; e2[2] = -e2[2]; } } @@ -1278,23 +1326,22 @@ static void mat3_to_eulo2(float M[3][3], float *e1, float *e2, short order) void eulO_to_mat4(float M[4][4], const float e[3], const short order) { float m[3][3]; - + /* for now, we'll just do this the slow way (i.e. copying matrices) */ normalize_m3(m); - eulO_to_mat3(m,e, order); + eulO_to_mat3(m, e, order); copy_m4_m3(M, m); } - /* Convert 3x3 matrix to Euler angles (in radians). */ -void mat3_to_eulO(float eul[3], const short order,float M[3][3]) +void mat3_to_eulO(float eul[3], const short order, float M[3][3]) { float eul1[3], eul2[3]; - + mat3_to_eulo2(M, eul1, eul2, order); - + /* return best, which is just the one with lowest values it in */ - if (fabs(eul1[0])+fabs(eul1[1])+fabs(eul1[2]) > fabs(eul2[0])+fabs(eul2[1])+fabs(eul2[2])) { + if (fabs(eul1[0]) + fabs(eul1[1]) + fabs(eul1[2]) > fabs(eul2[0]) + fabs(eul2[1]) + fabs(eul2[2])) { copy_v3_v3(eul, eul2); } else { @@ -1303,30 +1350,30 @@ void mat3_to_eulO(float eul[3], const short order,float M[3][3]) } /* Convert 4x4 matrix to Euler angles (in radians). */ -void mat4_to_eulO(float e[3], const short order,float M[4][4]) +void mat4_to_eulO(float e[3], const short order, float M[4][4]) { float m[3][3]; - + /* for now, we'll just do this the slow way (i.e. copying matrices) */ copy_m3_m4(m, M); normalize_m3(m); - mat3_to_eulO(e, order,m); + mat3_to_eulO(e, order, m); } /* uses 2 methods to retrieve eulers, and picks the closest */ -void mat3_to_compatible_eulO(float eul[3], float oldrot[3], short order,float mat[3][3]) +void mat3_to_compatible_eulO(float eul[3], float oldrot[3], short order, float mat[3][3]) { float eul1[3], eul2[3]; float d1, d2; - + mat3_to_eulo2(mat, eul1, eul2, order); - + compatible_eul(eul1, oldrot); compatible_eul(eul2, oldrot); - - d1= fabsf(eul1[0]-oldrot[0]) + fabsf(eul1[1]-oldrot[1]) + fabsf(eul1[2]-oldrot[2]); - d2= fabsf(eul2[0]-oldrot[0]) + fabsf(eul2[1]-oldrot[1]) + fabsf(eul2[2]-oldrot[2]); - + + d1 = fabsf(eul1[0] - oldrot[0]) + fabsf(eul1[1] - oldrot[1]) + fabsf(eul1[2] - oldrot[2]); + d2 = fabsf(eul2[0] - oldrot[0]) + fabsf(eul2[1] - oldrot[1]) + fabsf(eul2[2] - oldrot[2]); + /* return best, which is just the one with lowest difference */ if (d1 > d2) copy_v3_v3(eul, eul2); @@ -1334,10 +1381,10 @@ void mat3_to_compatible_eulO(float eul[3], float oldrot[3], short order,float ma copy_v3_v3(eul, eul1); } -void mat4_to_compatible_eulO(float eul[3], float oldrot[3], short order,float M[4][4]) +void mat4_to_compatible_eulO(float eul[3], float oldrot[3], short order, float M[4][4]) { float m[3][3]; - + /* for now, we'll just do this the slow way (i.e. copying matrices) */ copy_m3_m4(m, M); normalize_m3(m); @@ -1345,47 +1392,48 @@ void mat4_to_compatible_eulO(float eul[3], float oldrot[3], short order,float M[ } /* rotate the given euler by the given angle on the specified axis */ // NOTE: is this safe to do with different axis orders? + void rotate_eulO(float beul[3], short order, char axis, float ang) { float eul[3], mat1[3][3], mat2[3][3], totmat[3][3]; assert(axis >= 'X' && axis <= 'Z'); - eul[0]= eul[1]= eul[2]= 0.0f; - if (axis=='X') - eul[0]= ang; - else if (axis=='Y') - eul[1]= ang; - else - eul[2]= ang; - - eulO_to_mat3(mat1,eul, order); - eulO_to_mat3(mat2,beul, order); - + eul[0] = eul[1] = eul[2] = 0.0f; + if (axis == 'X') + eul[0] = ang; + else if (axis == 'Y') + eul[1] = ang; + else + eul[2] = ang; + + eulO_to_mat3(mat1, eul, order); + eulO_to_mat3(mat2, beul, order); + mul_m3_m3m3(totmat, mat2, mat1); - - mat3_to_eulO(beul, order,totmat); + + mat3_to_eulO(beul, order, totmat); } /* the matrix is written to as 3 axis vectors */ void eulO_to_gimbal_axis(float gmat[][3], const float eul[3], const short order) { - RotOrderInfo *R= GET_ROTATIONORDER_INFO(order); + RotOrderInfo *R = GET_ROTATIONORDER_INFO(order); float mat[3][3]; float teul[3]; /* first axis is local */ - eulO_to_mat3(mat,eul, order); + eulO_to_mat3(mat, eul, order); copy_v3_v3(gmat[R->axis[0]], mat[R->axis[0]]); - + /* second axis is local minus first rotation */ copy_v3_v3(teul, eul); teul[R->axis[0]] = 0; - eulO_to_mat3(mat,teul, order); + eulO_to_mat3(mat, teul, order); copy_v3_v3(gmat[R->axis[1]], mat[R->axis[1]]); - - + + /* Last axis is global */ gmat[R->axis[2]][0] = 0; gmat[R->axis[2]][1] = 0; @@ -1401,7 +1449,7 @@ void eulO_to_gimbal_axis(float gmat[][3], const float eul[3], const short order) * * Version 1.0.0, February 7th, 2007 * - * Copyright (C) 2006-2007 University of Dublin, Trinity College, All Rights + * Copyright (C) 2006-2007 University of Dublin, Trinity College, All Rights * Reserved * * This software is provided 'as-is', without any express or implied @@ -1427,7 +1475,7 @@ void eulO_to_gimbal_axis(float gmat[][3], const float eul[3], const short order) * - added support for scaling */ -void mat4_to_dquat(DualQuat *dq,float basemat[][4], float mat[][4]) +void mat4_to_dquat(DualQuat *dq, float basemat[][4], float mat[][4]) { float *t, *q, dscale[3], scale[3], basequat[4]; float baseRS[4][4], baseinv[4][4], baseR[4][4], baseRinv[4][4]; @@ -1436,16 +1484,18 @@ void mat4_to_dquat(DualQuat *dq,float basemat[][4], float mat[][4]) /* split scaling and rotation, there is probably a faster way to do * this, it's done like this now to correctly get negative scaling */ mult_m4_m4m4(baseRS, mat, basemat); - mat4_to_size(scale,baseRS); + mat4_to_size(scale, baseRS); copy_v3_v3(dscale, scale); - dscale[0] -= 1.0f; dscale[1] -= 1.0f; dscale[2] -= 1.0f; + dscale[0] -= 1.0f; + dscale[1] -= 1.0f; + dscale[2] -= 1.0f; if ((determinant_m4(mat) < 0.0f) || len_v3(dscale) > 1e-4f) { /* extract R and S */ float tmp[4][4]; - /* extra orthogonalize, to avoid flipping with stretched bones */ + /* extra orthogonalize, to avoid flipping with stretched bones */ copy_m4_m4(tmp, baseRS); orthogonalize_m4(tmp, 1); mat4_to_quat(basequat, tmp); @@ -1461,78 +1511,78 @@ void mat4_to_dquat(DualQuat *dq,float basemat[][4], float mat[][4]) /* set scaling part */ mul_serie_m4(dq->scale, basemat, S, baseinv, NULL, NULL, NULL, NULL, NULL); - dq->scale_weight= 1.0f; + dq->scale_weight = 1.0f; } else { /* matrix does not contain scaling */ copy_m4_m4(R, mat); - dq->scale_weight= 0.0f; + dq->scale_weight = 0.0f; } /* non-dual part */ - mat4_to_quat(dq->quat,R); + mat4_to_quat(dq->quat, R); /* dual part */ - t= R[3]; - q= dq->quat; - dq->trans[0]= -0.5f*(t[0]*q[1] + t[1]*q[2] + t[2]*q[3]); - dq->trans[1]= 0.5f*(t[0]*q[0] + t[1]*q[3] - t[2]*q[2]); - dq->trans[2]= 0.5f*(-t[0]*q[3] + t[1]*q[0] + t[2]*q[1]); - dq->trans[3]= 0.5f*(t[0]*q[2] - t[1]*q[1] + t[2]*q[0]); + t = R[3]; + q = dq->quat; + dq->trans[0] = -0.5f * (t[0] * q[1] + t[1] * q[2] + t[2] * q[3]); + dq->trans[1] = 0.5f * (t[0] * q[0] + t[1] * q[3] - t[2] * q[2]); + dq->trans[2] = 0.5f * (-t[0] * q[3] + t[1] * q[0] + t[2] * q[1]); + dq->trans[3] = 0.5f * (t[0] * q[2] - t[1] * q[1] + t[2] * q[0]); } void dquat_to_mat4(float mat[][4], DualQuat *dq) { float len, *t, q0[4]; - + /* regular quaternion */ copy_qt_qt(q0, dq->quat); /* normalize */ - len= (float)sqrt(dot_qtqt(q0, q0)); + len = (float)sqrt(dot_qtqt(q0, q0)); if (len != 0.0f) - mul_qt_fl(q0, 1.0f/len); - + mul_qt_fl(q0, 1.0f / len); + /* rotation */ - quat_to_mat4(mat,q0); + quat_to_mat4(mat, q0); /* translation */ - t= dq->trans; - mat[3][0]= 2.0f*(-t[0]*q0[1] + t[1]*q0[0] - t[2]*q0[3] + t[3]*q0[2]); - mat[3][1]= 2.0f*(-t[0]*q0[2] + t[1]*q0[3] + t[2]*q0[0] - t[3]*q0[1]); - mat[3][2]= 2.0f*(-t[0]*q0[3] - t[1]*q0[2] + t[2]*q0[1] + t[3]*q0[0]); + t = dq->trans; + mat[3][0] = 2.0f * (-t[0] * q0[1] + t[1] * q0[0] - t[2] * q0[3] + t[3] * q0[2]); + mat[3][1] = 2.0f * (-t[0] * q0[2] + t[1] * q0[3] + t[2] * q0[0] - t[3] * q0[1]); + mat[3][2] = 2.0f * (-t[0] * q0[3] - t[1] * q0[2] + t[2] * q0[1] + t[3] * q0[0]); /* note: this does not handle scaling */ -} +} void add_weighted_dq_dq(DualQuat *dqsum, DualQuat *dq, float weight) { - int flipped= 0; + int flipped = 0; /* make sure we interpolate quats in the right direction */ if (dot_qtqt(dq->quat, dqsum->quat) < 0) { - flipped= 1; - weight= -weight; + flipped = 1; + weight = -weight; } /* interpolate rotation and translation */ - dqsum->quat[0] += weight*dq->quat[0]; - dqsum->quat[1] += weight*dq->quat[1]; - dqsum->quat[2] += weight*dq->quat[2]; - dqsum->quat[3] += weight*dq->quat[3]; + dqsum->quat[0] += weight * dq->quat[0]; + dqsum->quat[1] += weight * dq->quat[1]; + dqsum->quat[2] += weight * dq->quat[2]; + dqsum->quat[3] += weight * dq->quat[3]; - dqsum->trans[0] += weight*dq->trans[0]; - dqsum->trans[1] += weight*dq->trans[1]; - dqsum->trans[2] += weight*dq->trans[2]; - dqsum->trans[3] += weight*dq->trans[3]; + dqsum->trans[0] += weight * dq->trans[0]; + dqsum->trans[1] += weight * dq->trans[1]; + dqsum->trans[2] += weight * dq->trans[2]; + dqsum->trans[3] += weight * dq->trans[3]; /* interpolate scale - but only if needed */ if (dq->scale_weight) { float wmat[4][4]; - - if (flipped) /* we don't want negative weights for scaling */ - weight= -weight; - + + if (flipped) /* we don't want negative weights for scaling */ + weight = -weight; + copy_m4_m4(wmat, dq->scale); mul_m4_fl(wmat, weight); add_m4_m4m4(dqsum->scale, dqsum->scale, wmat); @@ -1542,14 +1592,14 @@ void add_weighted_dq_dq(DualQuat *dqsum, DualQuat *dq, float weight) void normalize_dq(DualQuat *dq, float totweight) { - float scale= 1.0f/totweight; + float scale = 1.0f / totweight; mul_qt_fl(dq->quat, scale); mul_qt_fl(dq->trans, scale); - + if (dq->scale_weight) { - float addweight= totweight - dq->scale_weight; - + float addweight = totweight - dq->scale_weight; + if (addweight) { dq->scale[0][0] += addweight; dq->scale[1][1] += addweight; @@ -1558,47 +1608,47 @@ void normalize_dq(DualQuat *dq, float totweight) } mul_m4_fl(dq->scale, scale); - dq->scale_weight= 1.0f; + dq->scale_weight = 1.0f; } } -void mul_v3m3_dq(float *co, float mat[][3],DualQuat *dq) -{ +void mul_v3m3_dq(float *co, float mat[][3], DualQuat *dq) +{ float M[3][3], t[3], scalemat[3][3], len2; - float w= dq->quat[0], x= dq->quat[1], y= dq->quat[2], z= dq->quat[3]; - float t0= dq->trans[0], t1= dq->trans[1], t2= dq->trans[2], t3= dq->trans[3]; - + float w = dq->quat[0], x = dq->quat[1], y = dq->quat[2], z = dq->quat[3]; + float t0 = dq->trans[0], t1 = dq->trans[1], t2 = dq->trans[2], t3 = dq->trans[3]; + /* rotation matrix */ - M[0][0]= w*w + x*x - y*y - z*z; - M[1][0]= 2*(x*y - w*z); - M[2][0]= 2*(x*z + w*y); - - M[0][1]= 2*(x*y + w*z); - M[1][1]= w*w + y*y - x*x - z*z; - M[2][1]= 2*(y*z - w*x); - - M[0][2]= 2*(x*z - w*y); - M[1][2]= 2*(y*z + w*x); - M[2][2]= w*w + z*z - x*x - y*y; - - len2= dot_qtqt(dq->quat, dq->quat); + M[0][0] = w * w + x * x - y * y - z * z; + M[1][0] = 2 * (x * y - w * z); + M[2][0] = 2 * (x * z + w * y); + + M[0][1] = 2 * (x * y + w * z); + M[1][1] = w * w + y * y - x * x - z * z; + M[2][1] = 2 * (y * z - w * x); + + M[0][2] = 2 * (x * z - w * y); + M[1][2] = 2 * (y * z + w * x); + M[2][2] = w * w + z * z - x * x - y * y; + + len2 = dot_qtqt(dq->quat, dq->quat); if (len2 > 0.0f) - len2= 1.0f/len2; - + len2 = 1.0f / len2; + /* translation */ - t[0]= 2*(-t0*x + w*t1 - t2*z + y*t3); - t[1]= 2*(-t0*y + t1*z - x*t3 + w*t2); - t[2]= 2*(-t0*z + x*t2 + w*t3 - t1*y); + t[0] = 2 * (-t0 * x + w * t1 - t2 * z + y * t3); + t[1] = 2 * (-t0 * y + t1 * z - x * t3 + w * t2); + t[2] = 2 * (-t0 * z + x * t2 + w * t3 - t1 * y); /* apply scaling */ if (dq->scale_weight) mul_m4_v3(dq->scale, co); - + /* apply rotation and translation */ mul_m3_v3(M, co); - co[0]= (co[0] + t[0])*len2; - co[1]= (co[1] + t[1])*len2; - co[2]= (co[2] + t[2])*len2; + co[0] = (co[0] + t[0]) * len2; + co[1] = (co[1] + t[1]) * len2; + co[2] = (co[2] + t[2]) * len2; /* compute crazyspace correction mat */ if (mat) { @@ -1619,85 +1669,85 @@ void copy_dq_dq(DualQuat *dq1, DualQuat *dq2) /* axis matches eTrackToAxis_Modes */ void quat_apply_track(float quat[4], short axis, short upflag) -{ +{ /* rotations are hard coded to match vec_to_quat */ - const float quat_track[][4]= {{0.70710676908493, 0.0, -0.70710676908493, 0.0}, /* pos-y90 */ - {0.5, 0.5, 0.5, 0.5}, /* Quaternion((1,0,0), radians(90)) * Quaternion((0,1,0), radians(90)) */ - {0.70710676908493, 0.0, 0.0, 0.70710676908493}, /* pos-z90 */ - {0.70710676908493, 0.0, 0.70710676908493, 0.0}, /* neg-y90 */ - {0.5, -0.5, -0.5, 0.5}, /* Quaternion((1,0,0), radians(-90)) * Quaternion((0,1,0), radians(-90)) */ - {-3.0908619663705394e-08, 0.70710676908493, 0.70710676908493, 3.0908619663705394e-08}}; /* no rotation */ + const float quat_track[][4] = { + {0.70710676908493, 0.0, -0.70710676908493, 0.0}, /* pos-y90 */ + {0.5, 0.5, 0.5, 0.5}, /* Quaternion((1,0,0), radians(90)) * Quaternion((0,1,0), radians(90)) */ + {0.70710676908493, 0.0, 0.0, 0.70710676908493}, /* pos-z90 */ + {0.70710676908493, 0.0, 0.70710676908493, 0.0}, /* neg-y90 */ + {0.5, -0.5, -0.5, 0.5}, /* Quaternion((1,0,0), radians(-90)) * Quaternion((0,1,0), radians(-90)) */ + {-3.0908619663705394e-08, 0.70710676908493, 0.70710676908493, 3.0908619663705394e-08}}; /* no rotation */ assert(axis >= 0 && axis <= 5); assert(upflag >= 0 && upflag <= 2); - + mul_qt_qtqt(quat, quat, quat_track[axis]); - if (axis>2) - axis= axis-3; + if (axis > 2) + axis = axis - 3; /* there are 2 possible up-axis for each axis used, the 'quat_track' applies so the first * up axis is used X->Y, Y->X, Z->X, if this first up axis isn used then rotate 90d * the strange bit shift below just find the low axis {X:Y, Y:X, Z:X} */ - if (upflag != (2-axis)>>1) { - float q[4]= {0.70710676908493, 0.0, 0.0, 0.0}; /* assign 90d rotation axis */ - q[axis+1] = ((axis==1)) ? 0.70710676908493 : -0.70710676908493; /* flip non Y axis */ + if (upflag != (2 - axis) >> 1) { + float q[4] = {0.70710676908493, 0.0, 0.0, 0.0}; /* assign 90d rotation axis */ + q[axis + 1] = ((axis == 1)) ? 0.70710676908493 : -0.70710676908493; /* flip non Y axis */ mul_qt_qtqt(quat, quat, q); } } - void vec_apply_track(float vec[3], short axis) { float tvec[3]; assert(axis >= 0 && axis <= 5); - + copy_v3_v3(tvec, vec); - switch(axis) { - case 0: /* pos-x */ - /* vec[0]= 0.0; */ - vec[1]= tvec[2]; - vec[2]= -tvec[1]; - break; - case 1: /* pos-y */ - /* vec[0]= tvec[0]; */ - /* vec[1]= 0.0; */ - /* vec[2]= tvec[2]; */ - break; - case 2: /* pos-z */ - /* vec[0]= tvec[0]; */ - /* vec[1]= tvec[1]; */ - // vec[2]= 0.0; */ - break; - case 3: /* neg-x */ - /* vec[0]= 0.0; */ - vec[1]= tvec[2]; - vec[2]= -tvec[1]; - break; - case 4: /* neg-y */ - vec[0]= -tvec[2]; - /* vec[1]= 0.0; */ - vec[2]= tvec[0]; - break; - case 5: /* neg-z */ - vec[0]= -tvec[0]; - vec[1]= -tvec[1]; - /* vec[2]= 0.0; */ - break; + switch (axis) { + case 0: /* pos-x */ + /* vec[0]= 0.0; */ + vec[1] = tvec[2]; + vec[2] = -tvec[1]; + break; + case 1: /* pos-y */ + /* vec[0]= tvec[0]; */ + /* vec[1]= 0.0; */ + /* vec[2]= tvec[2]; */ + break; + case 2: /* pos-z */ + /* vec[0]= tvec[0]; */ + /* vec[1]= tvec[1]; */ + // vec[2]= 0.0; */ + break; + case 3: /* neg-x */ + /* vec[0]= 0.0; */ + vec[1] = tvec[2]; + vec[2] = -tvec[1]; + break; + case 4: /* neg-y */ + vec[0] = -tvec[2]; + /* vec[1]= 0.0; */ + vec[2] = tvec[0]; + break; + case 5: /* neg-z */ + vec[0] = -tvec[0]; + vec[1] = -tvec[1]; + /* vec[2]= 0.0; */ + break; } } /* lens/angle conversion (radians) */ float focallength_to_fov(float focal_length, float sensor) { - return 2.0f * atanf((sensor/2.0f) / focal_length); + return 2.0f * atanf((sensor / 2.0f) / focal_length); } float fov_to_focallength(float hfov, float sensor) { - return (sensor/2.0f) / tanf(hfov * 0.5f); + return (sensor / 2.0f) / tanf(hfov * 0.5f); } /* 'mod_inline(-3,4)= 1', 'fmod(-3,4)= -3' */ @@ -1708,7 +1758,7 @@ static float mod_inline(float a, float b) float angle_wrap_rad(float angle) { - return mod_inline(angle + (float)M_PI, (float)M_PI*2.0f) - (float)M_PI; + return mod_inline(angle + (float)M_PI, (float)M_PI * 2.0f) - (float)M_PI; } float angle_wrap_deg(float angle) diff --git a/source/blender/blenlib/intern/math_vector.c b/source/blender/blenlib/intern/math_vector.c index 63094d9cfd0..fcab5e75f39 100644 --- a/source/blender/blenlib/intern/math_vector.c +++ b/source/blender/blenlib/intern/math_vector.c @@ -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,7 +280,7 @@ 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]; @@ -321,7 +321,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 +329,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 +346,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 +390,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 +435,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 +454,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; + } } diff --git a/source/blender/blenlib/intern/math_vector_inline.c b/source/blender/blenlib/intern/math_vector_inline.c index 9c26c1ee79a..f1ab77a10e9 100644 --- a/source/blender/blenlib/intern/math_vector_inline.c +++ b/source/blender/blenlib/intern/math_vector_inline.c @@ -37,131 +37,131 @@ MINLINE void zero_v2(float r[2]) { - r[0]= 0.0f; - r[1]= 0.0f; + 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; + r[0] = 0.0f; + r[1] = 0.0f; + r[2] = 0.0f; } MINLINE void zero_v4(float r[4]) { - r[0]= 0.0f; - r[1]= 0.0f; - r[2]= 0.0f; - r[3]= 0.0f; + r[0] = 0.0f; + r[1] = 0.0f; + r[2] = 0.0f; + r[3] = 0.0f; } MINLINE void copy_v2_v2(float r[2], const float a[2]) { - r[0]= a[0]; - r[1]= a[1]; + r[0] = a[0]; + r[1] = a[1]; } MINLINE void copy_v3_v3(float r[3], const float a[3]) { - r[0]= a[0]; - r[1]= a[1]; - r[2]= a[2]; + r[0] = a[0]; + r[1] = a[1]; + r[2] = a[2]; } MINLINE void copy_v4_v4(float r[4], const float a[4]) { - r[0]= a[0]; - r[1]= a[1]; - r[2]= a[2]; - r[3]= a[3]; + r[0] = a[0]; + r[1] = a[1]; + r[2] = a[2]; + r[3] = a[3]; } MINLINE void copy_v2_fl(float r[2], float f) { - r[0]= f; - r[1]= f; + r[0] = f; + r[1] = f; } MINLINE void copy_v3_fl(float r[3], float f) { - r[0]= f; - r[1]= f; - r[2]= f; + r[0] = f; + r[1] = f; + r[2] = f; } MINLINE void copy_v4_fl(float r[4], float f) { - r[0]= f; - r[1]= f; - r[2]= f; - r[3]= f; + r[0] = f; + r[1] = f; + r[2] = f; + r[3] = f; } /* short */ MINLINE void copy_v2_v2_char(char r[2], const char a[2]) { - r[0]= a[0]; - r[1]= a[1]; + r[0] = a[0]; + r[1] = a[1]; } MINLINE void copy_v3_v3_char(char r[3], const char a[3]) { - r[0]= a[0]; - r[1]= a[1]; - r[2]= a[2]; + r[0] = a[0]; + r[1] = a[1]; + r[2] = a[2]; } MINLINE void copy_v4_v4_char(char r[4], const char a[4]) { - r[0]= a[0]; - r[1]= a[1]; - r[2]= a[2]; - r[3]= a[3]; + r[0] = a[0]; + r[1] = a[1]; + r[2] = a[2]; + r[3] = a[3]; } /* short */ MINLINE void copy_v2_v2_short(short r[2], const short a[2]) { - r[0]= a[0]; - r[1]= a[1]; + r[0] = a[0]; + r[1] = a[1]; } MINLINE void copy_v3_v3_short(short r[3], const short a[3]) { - r[0]= a[0]; - r[1]= a[1]; - r[2]= a[2]; + r[0] = a[0]; + r[1] = a[1]; + r[2] = a[2]; } MINLINE void copy_v4_v4_short(short r[4], const short a[4]) { - r[0]= a[0]; - r[1]= a[1]; - r[2]= a[2]; - r[3]= a[3]; + r[0] = a[0]; + r[1] = a[1]; + r[2] = a[2]; + r[3] = a[3]; } /* int */ MINLINE void copy_v2_v2_int(int r[2], const int a[2]) { - r[0]= a[0]; - r[1]= a[1]; + r[0] = a[0]; + r[1] = a[1]; } MINLINE void copy_v3_v3_int(int r[3], const int a[3]) { - r[0]= a[0]; - r[1]= a[1]; - r[2]= a[2]; + r[0] = a[0]; + r[1] = a[1]; + r[2] = a[2]; } MINLINE void copy_v4_v4_int(int r[4], const int a[4]) { - r[0]= a[0]; - r[1]= a[1]; - r[2]= a[2]; - r[3]= a[3]; + r[0] = a[0]; + r[1] = a[1]; + r[2] = a[2]; + r[3] = a[3]; } /* double -> float */ @@ -208,7 +208,6 @@ MINLINE void copy_v4db_v4fl(double r[4], const float a[4]) r[3] = (double)a[3]; } - MINLINE void swap_v2_v2(float a[2], float b[2]) { SWAP(float, a[0], b[0]); @@ -255,8 +254,8 @@ MINLINE void add_v2_v2(float r[2], const float a[2]) MINLINE void add_v2_v2v2(float r[2], const float a[2], const float b[2]) { - r[0]= a[0] + b[0]; - r[1]= a[1] + b[1]; + r[0] = a[0] + b[0]; + r[1] = a[1] + b[1]; } MINLINE void add_v3_v3(float r[3], const float a[3]) @@ -268,9 +267,9 @@ MINLINE void add_v3_v3(float r[3], const float a[3]) MINLINE void add_v3_v3v3(float r[3], const float a[3], const float b[3]) { - r[0]= a[0] + b[0]; - r[1]= a[1] + b[1]; - r[2]= a[2] + b[2]; + 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[2], const float a[2]) @@ -281,8 +280,8 @@ MINLINE void sub_v2_v2(float r[2], const float a[2]) MINLINE void sub_v2_v2v2(float r[2], const float a[2], const float b[2]) { - r[0]= a[0] - b[0]; - r[1]= a[1] - b[1]; + r[0] = a[0] - b[0]; + r[1] = a[1] - b[1]; } MINLINE void sub_v3_v3(float r[3], const float a[3]) @@ -294,9 +293,9 @@ MINLINE void sub_v3_v3(float r[3], const float a[3]) MINLINE void sub_v3_v3v3(float r[3], const float a[3], const float b[3]) { - r[0]= a[0] - b[0]; - r[1]= a[1] - b[1]; - r[2]= a[2] - b[2]; + r[0] = a[0] - b[0]; + r[1] = a[1] - b[1]; + r[2] = a[2] - b[2]; } MINLINE void sub_v4_v4(float r[4], const float a[4]) @@ -309,23 +308,22 @@ MINLINE void sub_v4_v4(float r[4], const float a[4]) MINLINE void sub_v4_v4v4(float r[4], const float a[4], const float b[4]) { - r[0]= a[0] - b[0]; - r[1]= a[1] - b[1]; - r[2]= a[2] - b[2]; - r[3]= a[3] - b[3]; + r[0] = a[0] - b[0]; + r[1] = a[1] - b[1]; + r[2] = a[2] - b[2]; + r[3] = a[3] - b[3]; } - MINLINE void mul_v2_fl(float r[2], float f) { - r[0]*= f; - r[1]*= f; + r[0] *= f; + r[1] *= f; } MINLINE void mul_v2_v2fl(float r[2], const float a[2], float f) { - r[0]= a[0]*f; - r[1]= a[1]*f; + r[0] = a[0] * f; + r[1] = a[1] * f; } MINLINE void mul_v3_fl(float r[3], float f) @@ -337,9 +335,9 @@ MINLINE void mul_v3_fl(float r[3], float f) MINLINE void mul_v3_v3fl(float r[3], const float a[3], float f) { - r[0]= a[0]*f; - r[1]= a[1]*f; - r[2]= a[2]*f; + r[0] = a[0] * f; + r[1] = a[1] * f; + r[2] = a[2] * f; } MINLINE void mul_v2_v2(float r[2], const float a[2]) @@ -357,58 +355,58 @@ MINLINE void mul_v3_v3(float r[3], const float a[3]) MINLINE void mul_v4_fl(float r[4], float f) { - r[0]*= f; - r[1]*= f; - r[2]*= f; - r[3]*= f; + r[0] *= f; + r[1] *= f; + r[2] *= f; + r[3] *= f; } MINLINE void madd_v2_v2fl(float r[2], const float a[2], float f) { - r[0] += a[0]*f; - r[1] += a[1]*f; + r[0] += a[0] * f; + r[1] += a[1] * f; } MINLINE void madd_v3_v3fl(float r[3], const float a[3], float f) { - r[0] += a[0]*f; - r[1] += a[1]*f; - r[2] += a[2]*f; + r[0] += a[0] * f; + r[1] += a[1] * f; + r[2] += a[2] * f; } MINLINE void madd_v3_v3v3(float r[3], const float a[3], const float b[3]) { - r[0] += a[0]*b[0]; - r[1] += a[1]*b[1]; - r[2] += a[2]*b[2]; + r[0] += a[0] * b[0]; + r[1] += a[1] * b[1]; + r[2] += a[2] * b[2]; } MINLINE void madd_v2_v2v2fl(float r[2], const float a[2], const float b[2], float f) { - r[0] = a[0] + b[0]*f; - r[1] = a[1] + b[1]*f; + r[0] = a[0] + b[0] * f; + r[1] = a[1] + b[1] * f; } MINLINE void madd_v3_v3v3fl(float r[3], const float a[3], const float b[3], float f) { - r[0] = a[0] + b[0]*f; - r[1] = a[1] + b[1]*f; - r[2] = a[2] + b[2]*f; + r[0] = a[0] + b[0] * f; + r[1] = a[1] + b[1] * f; + r[2] = a[2] + b[2] * f; } MINLINE void madd_v3_v3v3v3(float r[3], const float a[3], const float b[3], const float c[3]) { - r[0] = a[0] + b[0]*c[0]; - r[1] = a[1] + b[1]*c[1]; - r[2] = a[2] + b[2]*c[2]; + r[0] = a[0] + b[0] * c[0]; + r[1] = a[1] + b[1] * c[1]; + r[2] = a[2] + b[2] * c[2]; } MINLINE void madd_v4_v4fl(float r[4], const float a[4], float f) { - r[0] += a[0]*f; - r[1] += a[1]*f; - r[2] += a[2]*f; - r[3] += a[3]*f; + r[0] += a[0] * f; + r[1] += a[1] * f; + r[2] += a[2] * f; + r[3] += a[3] * f; } MINLINE void mul_v3_v3v3(float r[3], const float v1[3], const float v2[3]) @@ -420,98 +418,98 @@ MINLINE void mul_v3_v3v3(float r[3], const float v1[3], const float v2[3]) MINLINE void negate_v2(float r[3]) { - r[0]= -r[0]; - r[1]= -r[1]; + r[0] = -r[0]; + r[1] = -r[1]; } MINLINE void negate_v2_v2(float r[2], const float a[2]) { - r[0]= -a[0]; - r[1]= -a[1]; + r[0] = -a[0]; + r[1] = -a[1]; } MINLINE void negate_v3(float r[3]) { - r[0]= -r[0]; - r[1]= -r[1]; - r[2]= -r[2]; + r[0] = -r[0]; + r[1] = -r[1]; + r[2] = -r[2]; } MINLINE void negate_v3_v3(float r[3], const float a[3]) { - r[0]= -a[0]; - r[1]= -a[1]; - r[2]= -a[2]; + r[0] = -a[0]; + r[1] = -a[1]; + r[2] = -a[2]; } MINLINE void negate_v4(float r[4]) { - r[0]= -r[0]; - r[1]= -r[1]; - r[2]= -r[2]; - r[3]= -r[3]; + r[0] = -r[0]; + r[1] = -r[1]; + r[2] = -r[2]; + r[3] = -r[3]; } MINLINE void negate_v4_v4(float r[4], const float a[4]) { - r[0]= -a[0]; - r[1]= -a[1]; - r[2]= -a[2]; - r[3]= -a[3]; + r[0] = -a[0]; + r[1] = -a[1]; + r[2] = -a[2]; + r[3] = -a[3]; } MINLINE float dot_v2v2(const float a[2], const float b[2]) { - return a[0]*b[0] + a[1]*b[1]; + return a[0] * b[0] + a[1] * b[1]; } MINLINE float dot_v3v3(const float a[3], const float b[3]) { - return a[0]*b[0] + a[1]*b[1] + a[2]*b[2]; + return a[0] * b[0] + a[1] * b[1] + a[2] * b[2]; } MINLINE float cross_v2v2(const float a[2], const float b[2]) { - return a[0]*b[1] - a[1]*b[0]; + return a[0] * b[1] - a[1] * b[0]; } MINLINE void cross_v3_v3v3(float r[3], const float a[3], const 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]; + 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 rmat[][3], float a[3]) { - rmat[0][0]= rmat[1][1]= rmat[2][2]= 0.0; - rmat[0][1]= -a[2]; - rmat[0][2]= a[1]; - rmat[1][0]= a[2]; - rmat[1][2]= -a[0]; - rmat[2][0]= -a[1]; - rmat[2][1]= a[0]; + rmat[0][0] = rmat[1][1] = rmat[2][2] = 0.0; + rmat[0][1] = -a[2]; + rmat[0][2] = a[1]; + rmat[1][0] = a[2]; + rmat[1][2] = -a[0]; + rmat[2][0] = -a[1]; + rmat[2][1] = a[0]; } /*********************************** Length **********************************/ MINLINE float len_squared_v2(const float v[2]) { - return v[0]*v[0] + v[1]*v[1]; + return v[0] * v[0] + v[1] * v[1]; } MINLINE float len_v2(const float v[2]) { - return (float)sqrtf(v[0]*v[0] + v[1]*v[1]); + return (float)sqrtf(v[0] * v[0] + v[1] * v[1]); } MINLINE float len_v2v2(const float v1[2], const float v2[2]) { float x, y; - x = v1[0]-v2[0]; - y = v1[1]-v2[1]; - return (float)sqrtf(x*x+y*y); + x = v1[0] - v2[0]; + y = v1[1] - v2[1]; + return (float)sqrtf(x * x + y * y); } MINLINE float len_v3(const float a[3]) @@ -545,15 +543,15 @@ MINLINE float len_squared_v3v3(const float a[3], const float b[3]) MINLINE float normalize_v2_v2(float r[2], const float a[2]) { - float d= dot_v2v2(a, a); + float d = dot_v2v2(a, a); if (d > 1.0e-35f) { - d= sqrtf(d); - mul_v2_v2fl(r, a, 1.0f/d); + d = sqrtf(d); + mul_v2_v2fl(r, a, 1.0f / d); } else { zero_v2(r); - d= 0.0f; + d = 0.0f; } return d; @@ -566,17 +564,17 @@ MINLINE float normalize_v2(float n[2]) MINLINE float normalize_v3_v3(float r[3], const float a[3]) { - float d= dot_v3v3(a, a); + float d = dot_v3v3(a, a); /* 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_v3fl(r, a, 1.0f/d); + d = sqrtf(d); + mul_v3_v3fl(r, a, 1.0f / d); } else { zero_v3(r); - d= 0.0f; + d = 0.0f; } return d; @@ -584,14 +582,14 @@ MINLINE float normalize_v3_v3(float r[3], const float a[3]) MINLINE double normalize_v3_d(double n[3]) { - double d= n[0]*n[0] + n[1]*n[1] + n[2]*n[2]; + double d = n[0] * n[0] + n[1] * n[1] + n[2] * n[2]; /* a larger value causes normalize errors in a * scaled down models with camera xtreme close */ if (d > 1.0e-35) { double mul; - d= sqrt(d); + d = sqrt(d); mul = 1.0 / d; n[0] *= mul; @@ -600,7 +598,7 @@ MINLINE double normalize_v3_d(double n[3]) } else { n[0] = n[1] = n[2] = 0; - d= 0.0; + d = 0.0; } return d; @@ -613,16 +611,16 @@ MINLINE float normalize_v3(float n[3]) MINLINE void normal_short_to_float_v3(float out[3], const short in[3]) { - out[0] = in[0]*(1.0f/32767.0f); - out[1] = in[1]*(1.0f/32767.0f); - out[2] = in[2]*(1.0f/32767.0f); + out[0] = in[0] * (1.0f / 32767.0f); + out[1] = in[1] * (1.0f / 32767.0f); + out[2] = in[2] * (1.0f / 32767.0f); } MINLINE void normal_float_to_short_v3(short out[3], const float in[3]) { - out[0] = (short)(in[0]*32767.0f); - out[1] = (short)(in[1]*32767.0f); - out[2] = (short)(in[2]*32767.0f); + out[0] = (short) (in[0] * 32767.0f); + out[1] = (short) (in[1] * 32767.0f); + out[2] = (short) (in[2] * 32767.0f); } /********************************* Comparison ********************************/ @@ -650,24 +648,24 @@ MINLINE int is_one_v3(const float v[3]) MINLINE int equals_v2v2(const float v1[2], const float v2[2]) { - return ((v1[0]==v2[0]) && (v1[1]==v2[1])); + return ((v1[0] == v2[0]) && (v1[1] == v2[1])); } MINLINE int equals_v3v3(const float v1[3], const float v2[3]) { - return ((v1[0]==v2[0]) && (v1[1]==v2[1]) && (v1[2]==v2[2])); + return ((v1[0] == v2[0]) && (v1[1] == v2[1]) && (v1[2] == v2[2])); } MINLINE int equals_v4v4(const float v1[4], const float v2[4]) { - return ((v1[0]==v2[0]) && (v1[1]==v2[1]) && (v1[2]==v2[2]) && (v1[3]==v2[3])); + return ((v1[0] == v2[0]) && (v1[1] == v2[1]) && (v1[2] == v2[2]) && (v1[3] == v2[3])); } MINLINE int compare_v3v3(const float v1[3], const float v2[3], const float limit) { - if (fabsf(v1[0]-v2[0])<limit) - if (fabsf(v1[1]-v2[1])<limit) - if (fabsf(v1[2]-v2[2])<limit) + if (fabsf(v1[0] - v2[0]) < limit) + if (fabsf(v1[1] - v2[1]) < limit) + if (fabsf(v1[2] - v2[2]) < limit) return 1; return 0; @@ -675,21 +673,21 @@ MINLINE int compare_v3v3(const float v1[3], const float v2[3], const float limit MINLINE int compare_len_v3v3(const float v1[3], const float v2[3], const float limit) { - float x,y,z; + float x, y, z; - x=v1[0]-v2[0]; - y=v1[1]-v2[1]; - z=v1[2]-v2[2]; + x = v1[0] - v2[0]; + y = v1[1] - v2[1]; + z = v1[2] - v2[2]; - return ((x*x + y*y + z*z) < (limit*limit)); + return ((x * x + y * y + z * z) < (limit * limit)); } MINLINE int compare_v4v4(const float v1[4], const float v2[4], const float limit) { - if (fabsf(v1[0]-v2[0])<limit) - if (fabsf(v1[1]-v2[1])<limit) - if (fabsf(v1[2]-v2[2])<limit) - if (fabsf(v1[3]-v2[3])<limit) + if (fabsf(v1[0] - v2[0]) < limit) + if (fabsf(v1[1] - v2[1]) < limit) + if (fabsf(v1[2] - v2[2]) < limit) + if (fabsf(v1[3] - v2[3]) < limit) return 1; return 0; @@ -697,8 +695,8 @@ MINLINE int compare_v4v4(const float v1[4], const float v2[4], const float limit MINLINE float line_point_side_v2(const float l1[2], const float l2[2], const float pt[2]) { - return ((l1[0]-pt[0]) * (l2[1]-pt[1])) - - ((l2[0]-pt[0]) * (l1[1]-pt[1])); + return (((l1[0] - pt[0]) * (l2[1] - pt[1])) - + ((l2[0] - pt[0]) * (l1[1] - pt[1]))); } #endif /* __MATH_VECTOR_INLINE_C__ */ |