diff options
Diffstat (limited to 'src/shared/geom.cpp')
| -rw-r--r-- | src/shared/geom.cpp | 43 |
1 files changed, 12 insertions, 31 deletions
diff --git a/src/shared/geom.cpp b/src/shared/geom.cpp index a5192e4..ad71841 100644 --- a/src/shared/geom.cpp +++ b/src/shared/geom.cpp @@ -4,15 +4,13 @@ static inline double det2x2(double a, double b, double c, double d) { return a*d - b*c; } static inline double det3x3(double a1, double a2, double a3, double b1, double b2, double b3, - double c1, double c2, double c3) -{ + double c1, double c2, double c3) { return a1 * det2x2(b2, b3, c2, c3) - b1 * det2x2(a2, a3, c2, c3) + c1 * det2x2(a2, a3, b2, b3); } -bool matrix4::invert(const matrix4 &m, double mindet) -{ +bool matrix4::invert(const matrix4 &m, double mindet) { double a1 = m.a.x, a2 = m.a.y, a3 = m.a.z, a4 = m.a.w, b1 = m.b.x, b2 = m.b.y, b3 = m.b.z, b4 = m.b.w, c1 = m.c.x, c2 = m.c.y, c3 = m.c.z, c4 = m.c.w, @@ -22,36 +20,28 @@ bool matrix4::invert(const matrix4 &m, double mindet) det3 = det3x3(a2, a3, a4, b2, b3, b4, d2, d3, d4), det4 = -det3x3(a2, a3, a4, b2, b3, b4, c2, c3, c4), det = a1*det1 + b1*det2 + c1*det3 + d1*det4; - if(fabs(det) < mindet) return false; - double invdet = 1/det; - a.x = det1 * invdet; a.y = det2 * invdet; a.z = det3 * invdet; a.w = det4 * invdet; - b.x = -det3x3(b1, b3, b4, c1, c3, c4, d1, d3, d4) * invdet; b.y = det3x3(a1, a3, a4, c1, c3, c4, d1, d3, d4) * invdet; b.z = -det3x3(a1, a3, a4, b1, b3, b4, d1, d3, d4) * invdet; b.w = det3x3(a1, a3, a4, b1, b3, b4, c1, c3, c4) * invdet; - c.x = det3x3(b1, b2, b4, c1, c2, c4, d1, d2, d4) * invdet; c.y = -det3x3(a1, a2, a4, c1, c2, c4, d1, d2, d4) * invdet; c.z = det3x3(a1, a2, a4, b1, b2, b4, d1, d2, d4) * invdet; c.w = -det3x3(a1, a2, a4, b1, b2, b4, c1, c2, c4) * invdet; - d.x = -det3x3(b1, b2, b3, c1, c2, c3, d1, d2, d3) * invdet; d.y = det3x3(a1, a2, a3, c1, c2, c3, d1, d2, d3) * invdet; d.z = -det3x3(a1, a2, a3, b1, b2, b3, d1, d2, d3) * invdet; d.w = det3x3(a1, a2, a3, b1, b2, b3, c1, c2, c3) * invdet; - return true; } -bool raysphereintersect(const vec ¢er, float radius, const vec &o, const vec &ray, float &dist) -{ +bool raysphereintersect(const vec ¢er, float radius, const vec &o, const vec &ray, float &dist) { vec c(center); c.sub(o); float v = c.dot(ray), @@ -63,17 +53,14 @@ bool raysphereintersect(const vec ¢er, float radius, const vec &o, const vec return true; } -bool rayboxintersect(const vec &b, const vec &s, const vec &o, const vec &ray, float &dist, int &orient) -{ - loop(d, 3) if(ray[d]) - { +bool rayboxintersect(const vec &b, const vec &s, const vec &o, const vec &ray, float &dist, int &orient) { + loop(d, 3) if(ray[d]) { int dc = ray[d]<0 ? 1 : 0; float pdist = (b[d]+s[d]*dc - o[d]) / ray[d]; vec v(ray); v.mul(pdist).add(o); if(v[R[d]] >= b[R[d]] && v[R[d]] <= b[R[d]]+s[R[d]] - && v[C[d]] >= b[C[d]] && v[C[d]] <= b[C[d]]+s[C[d]]) - { + && v[C[d]] >= b[C[d]] && v[C[d]] <= b[C[d]]+s[C[d]]) { dist = pdist; orient = 2*d+dc; return true; @@ -82,8 +69,7 @@ bool rayboxintersect(const vec &b, const vec &s, const vec &o, const vec &ray, f return false; } -bool linecylinderintersect(const vec &from, const vec &to, const vec &start, const vec &end, float radius, float &dist) -{ +bool linecylinderintersect(const vec &from, const vec &to, const vec &start, const vec &end, float radius, float &dist) { vec d(end), m(from), n(to); d.sub(start); m.sub(start); @@ -98,16 +84,14 @@ bool linecylinderintersect(const vec &from, const vec &to, const vec &start, con a = dd*nn - nd*nd, k = m.squaredlen() - radius*radius, c = dd*k - md*md; - if(fabs(a) < 0.005f) - { + if(fabs(a) < 0.005f) { if(c > 0) return false; if(md < 0) dist = -mn / nn; else if(md > dd) dist = (nd - mn) / nn; else dist = 0; return true; } - else if(c > 0) - { + else if(c > 0) { float b = dd*mn - nd*md, discrim = b*b - a*c; if(discrim < 0) return false; @@ -115,14 +99,12 @@ bool linecylinderintersect(const vec &from, const vec &to, const vec &start, con } else dist = 0; float offset = md + dist*nd; - if(offset < 0) - { + if(offset < 0) { if(nd <= 0) return false; dist = -md / nd; if(k + dist*(2*mn + dist*nn) > 0) return false; } - else if(offset > dd) - { + else if(offset > dd) { if(nd >= 0) return false; dist = (dd - md) / nd; if(k + dd - 2*md + dist*(2*(mn-nd) + dist*nn) > 0) return false; @@ -130,8 +112,7 @@ bool linecylinderintersect(const vec &from, const vec &to, const vec &start, con return dist >= 0 && dist <= 1; } -extern const vec2 sincos360[721] = -{ +extern const vec2 sincos360[721] = { vec2(1.00000000, 0.00000000), vec2(0.99984770, 0.01745241), vec2(0.99939083, 0.03489950), vec2(0.99862953, 0.05233596), vec2(0.99756405, 0.06975647), vec2(0.99619470, 0.08715574), // 0 vec2(0.99452190, 0.10452846), vec2(0.99254615, 0.12186934), vec2(0.99026807, 0.13917310), vec2(0.98768834, 0.15643447), vec2(0.98480775, 0.17364818), vec2(0.98162718, 0.19080900), // 6 vec2(0.97814760, 0.20791169), vec2(0.97437006, 0.22495105), vec2(0.97029573, 0.24192190), vec2(0.96592583, 0.25881905), vec2(0.96126170, 0.27563736), vec2(0.95630476, 0.29237170), // 12 |