//============================================================================= bool Intersect (IntersectInfo3 & out, const Ray3 & r, const Aabb3 & b) { float32 & tmin = out.time; float32 tmax = std::numeric_limits<float32>::max(); tmin = 0; Vector3 nmin(1e-10f, 1e-11f, 1e-12f); static const Vector3 s_normals[] = { Vector3(1.0f, 0.0f, 0.0f), Vector3(0.0f, 1.0f, 0.0f), Vector3(0.0f, 0.0f, 1.0f) }; for (uint i = 0; i < 3; ++i) { // ray is parallel to slab? //if (Equal(r.direction[i], 0.0f)) if (Abs(r.direction[i]) < 0.0001f) { if (r.origin[i] < b.min[i] || r.origin[i] > b.max[i]) return false; } else { const float32 ood = 1.0f / r.direction[i]; float32 t1 = (b.min[i] - r.origin[i]) * ood; float32 t2 = (b.max[i] - r.origin[i]) * ood; const Vector3 & n1 = s_normals[i]; //Vector3 n2 = Vector3::Zero; n2[i] = +1.0f; float32 nSign = 1.0f; // make sure that t1 is always less than or equal to t2 if (t1 > t2) { std::swap(t1, t2); nSign = -1.0f; //std::swap(n1, n2); } if (t1 > tmin) { tmin = t1; nmin = n1 * nSign; } //tmin = Max(tmin, t1); tmax = Min(tmax, t2); if (tmin > tmax) return false; } } out.point = r.origin + tmin * r.direction; out.normal = nmin; return true; }
bool Box::intersectLocal( const ray& r, isect& i ) const { BoundingBox bounds = ComputeLocalBoundingBox(); vec3f p = r.getPosition(); vec3f d = r.getDirection(); //find tmin and tmax vec3f tmin; vec3f tmax; vec3f nmin(0, 0, 0); vec3f nmax(0, 0, 0); double min; double max; for(int j=0; j<3; j++) { if(d[j]>=0) { tmin[j] = (bounds.min[j] - p[j]) / d[j]; tmax[j] = (bounds.max[j] - p[j]) / d[j]; nmin[j] = -1; nmax[j] = 1; } else { tmin[j] = (bounds.max[j] - p[j]) / d[j]; tmax[j] = (bounds.min[j] - p[j]) / d[j]; nmin[j] = 1; nmax[j] = -1; } } //min of tmax, max of tmin max = std::min( std::min(tmax[0], tmax[1]), tmax[2]); min = std::max( std::max(tmin[0], tmin[1]), tmin[2]); if(min > max || max < RAY_EPSILON) return false; i.obj = this; vec3f N(0, 0, 0); if(min >= RAY_EPSILON) { i.t = min; for(int i=0; i<3; i++) { if(tmin[i] == min) { N[i] = nmin[i]; break; } } } else { i.t = max; for(int i=0; i<3; i++) { if(tmax[i] == max) { N[i] = nmax[i]; break; } } } i.N = N; return true; }
void collideWithPolyTrees(Particles& particles, const Matrix4& txMx, const std::vector<Object>& objs, std::vector<ozcollide::AABBTreePoly*>& coltrees) { Particles::Positions pos = particles.pos_; Particles::Velocities& vel = particles.vel_; Particles::Velocities& dv = particles.dv_; // Particles::Accelerations& da = particles.da_; const unsigned size = pos.size(); for (unsigned i=0;i<size;i++) { pos[i] = Matrix4AffineReal3(txMx, pos[i]); } for (unsigned i=0;i<size;i++) { Real3 pi = pos[i]; Real3 pf = pi+dv[i]; std::vector<ozcollide::AABBTreePoly::SegmentColResult> colres(coltrees.size()); for (unsigned j=0;j<coltrees.size();j++) { coltrees[j]->collideWithSegment(toVec3f(pi),toVec3f(pf), colres[j]); } for (unsigned j=0;j<coltrees.size();j++) { real tmin = std::numeric_limits<float>::max(); Real3 nmin(0.); const ozcollide::Polygon* pmin = 0; for (unsigned k=0;k<colres[j].polys_.size();k++) { const ozcollide::Polygon* poly= colres[j].polys_[k]; const Real3& a = objs[j].vx_[poly->getIndex(0)]; const Real3& b = objs[j].vx_[poly->getIndex(1)]; const Real3& c = objs[j].vx_[poly->getIndex(2)]; // const Real3& an = objs[j].vn_[poly->getIndex(0)]; // const Real3& bn = objs[j].vn_[poly->getIndex(1)]; // const Real3& cn = objs[j].vn_[poly->getIndex(2)]; ozcollide::Plane plane; plane.fromPoints(toVec3f(a),toVec3f(b), toVec3f(c) ); real t; if (plane.intersectWithLine(toVec3f(pi),toVec3f(pf),t)) { if(t<tmin) { tmin=t; pmin = poly; nmin = normalize( crossProd( b-a, c-a) ); } } // reaction if (pmin) { // Real3 dvi = dv[i]; // Real3 x = pi+tmin*dvi; // pos[i]=x+(1.-tmin)*(dvi.norm())*nmin; // separate from geometry dv[i] = tmin*dv[i]+(1.-tmin)*dv[i].norm()*nmin; // perfect bounce == 2 slip walls = 1 real k = 1.8; vel[i]+=-k*dotProd(vel[i],nmin)*nmin; // bounce vel[i]*=0.95; // this gets inside geometry often // pos[i]=x+(1.-tmin)*(dv[i].norm())*nmin; // vel[i]=vel[i].norm()*nmin; // dv[i]=Real3(0.); // da[i]= Real3(0.); } } } } }