Intersection intersectionRayTriangle(const VEC3& P, const VEC3& Dir, const VEC3& Ta, const VEC3& Tb, const VEC3& Tc, VEC3& Inter) { typedef typename VEC3::DATA_TYPE T ; VEC3 u = Ta - P ; VEC3 v = Tb - P ; VEC3 w = Tc - P ; T x = tripleProduct(Dir, u, v) ; T y = tripleProduct(Dir, v, w) ; T z = tripleProduct(Dir, w, u) ; // std::cout << x << " / "<< y << " / "<< z << std::endl; if((x < T(0) && y < T(0) && z < T(0)) || (x > T(0) && y > T(0) && z > T(0))) { T sum = x + y + z ; T alpha = y / sum ; T beta = z / sum ; T gamma = T(1) - alpha - beta ; Inter = Ta * alpha + Tb * beta + Tc * gamma ; return FACE_INTERSECTION ; } if( ( x == T(0) && y > T(0) && z > T(0) ) || ( x == T(0) && y < T(0) && z < T(0) ) || // intersection on [Ta,Tb] ( x > T(0) && y == T(0) && z > T(0) ) || ( x < T(0) && y == T(0) && z < T(0) ) || // intersection on [Tb,Tc] ( x > T(0) && y > T(0) && z == T(0) ) || ( x < T(0) && y < T(0) && z == T(0) ) // intersection on [Tc,Ta] ) return EDGE_INTERSECTION ; if( ( x == T(0) && y == T(0) && z != T(0) ) || // intersection on Tb ( x == T(0) && y != T(0) && z == T(0) ) || // intersection on Ta ( x != T(0) && y == T(0) && z == T(0) ) // intersection on Tc ) return VERTEX_INTERSECTION ; return NO_INTERSECTION ; }
Edge_t findClosestEdge(std::vector<MathVector> simplex) { Edge_t closest; closest.distance = std::numeric_limits<double>::max(); closest.index = 0; double d; MathVector a, b, e, oa, n; for(int i = 0; i < simplex.size(); i++) { a = simplex[i]; b = simplex[(i+1)%simplex.size()]; e = b.subtractVectors(a); oa = a; n = tripleProduct(e, oa , e); n = n.toUnit(); // std::cout << "normal triple product: " << n.getX() << ", " << n.getY() << std::endl; d = n.dotProduct(a); if(d < closest.distance) { closest.distance = d; closest.normal = n; closest.index = (i+1)%simplex.size(); } } return closest; }
void SteerLib::GJK_EPA::findNearestEdge(std::vector<Util::Vector> simplex, float& distance, Util::Vector& normal, int& index) { Util::Vector current_point; Util::Vector point_plus_one; Util::Vector e; Util::Vector n; Util::Vector n_norm; distance = FLT_MAX; int count; for (int count = 0; count < simplex.size(); count++) { int j = count + 1 == simplex.size() ? 0 : count + 1; current_point = simplex[count]; point_plus_one = simplex[j]; e = point_plus_one - current_point; n = tripleProduct(e, current_point, e); n_norm = n / n.norm(); double d = n_norm * current_point; if (d < distance) { distance = d; normal = n_norm; index = j; } } }
Intersection intersectionRayTriangleOpt(const VEC3& P, const VEC3& Dir, const VEC3& Ta, const VEC3& Tb, const VEC3& Tc, VEC3& Inter) { typedef typename VEC3::DATA_TYPE T ; VEC3 u = Ta - P ; VEC3 v = Tb - P ; VEC3 w = Tc - P ; T x = tripleProduct(Dir, u, v) ; T y = tripleProduct(Dir, v, w) ; T z = tripleProduct(Dir, w, u) ; unsigned int np = 0 ; unsigned int nn = 0 ; unsigned int nz = 0 ; if (x > T(0)) ++np ; else if (x < T(0)) ++nn ; else ++nz ; if (y > T(0)) ++np ; else if (y < T(0)) ++nn ; else ++nz ; if (z > T(0)) ++np ; else if (z < T(0)) ++nn ; else ++nz ; if ((np != 0) && (nn != 0)) return NO_INTERSECTION ; T sum = x + y + z ; T alpha = y / sum ; T beta = z / sum ; T gamma = T(1) - alpha - beta ; Inter = Ta * alpha + Tb * beta + Tc * gamma ; return Intersection(FACE_INTERSECTION - nz) ; }
bool containsOrigin(std::vector<MathVector> &simplex, MathVector &direction) { MathVector a = simplex.back(); MathVector b, c, ab, ac, abPerp, acPerp; MathVector ao = a.negate(); if(simplex.size() == 3) { b = simplex[0]; c = simplex[1]; ab = b.subtractVectors(a); ac = c.subtractVectors(a); abPerp = tripleProduct(ac, ab, ab); acPerp = tripleProduct(ab, ac, ac); if(abPerp.dotProduct(ao) > 0) { simplex.erase(simplex.begin() + 1); direction = abPerp; } else if (acPerp.dotProduct(ao) > 0) { simplex.erase(simplex.begin()); direction = acPerp; } else { return true; } } else { b = simplex[0]; ab = b.subtractVectors(a); abPerp = tripleProduct(ab, ao, ab); if(abPerp.getX() == 0 || abPerp.getY() == 0) { direction = ab.perpendicular(); } else { direction = abPerp; } } return false; }
Intersection intersectionRayTriangleOpt(const VEC3& P, const VEC3& Dir, const VEC3& Ta, const VEC3& Tb, const VEC3& Tc) { typedef typename VEC3::DATA_TYPE T ; VEC3 u = Ta - P ; VEC3 v = Tb - P ; VEC3 w = Tc - P ; T x = tripleProduct(Dir, u, v) ; T y = tripleProduct(Dir, v, w) ; T z = tripleProduct(Dir, w, u) ; unsigned int np = 0 ; unsigned int nn = 0 ; unsigned int nz = 0 ; if (x > T(0)) ++np ; else if (x < T(0)) ++nn ; else ++nz ; if (y > T(0)) ++np ; else if (y < T(0)) ++nn ; else ++nz ; if (z > T(0)) ++np ; else if (z < T(0)) ++nn ; else ++nz ; if ((np != 0) && (nn != 0)) return NO_INTERSECTION ; return Intersection(FACE_INTERSECTION - nz) ; }
bool SteerLib::GJK_EPA::EPA(std::vector<Util::Vector> shape1, std::vector<Util::Vector> shape2, std::vector<Util::Vector> simplex, float& depth, Util::Vector& mtv) { float DEPTHTOLERANCE = .01f; while(true) { //CONDITION? int index = simplex.size()-1; float penDepth; Util::Vector addSimplex; float closest; Util::Vector closestVector; Util::Vector edge; edge = simplex[index] - simplex[0]; closestVector = tripleProduct(edge, simplex[index], edge); //std::cout << closestVector; closestVector = closestVector / closestVector.norm(); //normalize vector //std::cout << closestVector << std::endl; closest = dotProduct(simplex[index], closestVector); for(int i=0; i < simplex.size()-1; i++) { //std::cout << simplex.size() << std::endl; //std::cout << simplex[i] << std::endl; Util::Vector testVector; float testDepth; edge = simplex[i+1] - simplex[i]; testVector = tripleProduct(edge, simplex[i], edge); //std::cout << testVector; testVector = testVector / testVector.norm(); //normalize vector //std::cout << testVector << std::endl; testDepth = dotProduct(simplex[i], testVector); if(testDepth < closest) { closest = testDepth; closestVector = testVector; index = i; } } addSimplex = getSupport(shape1, closestVector) - getSupport(shape2, closestVector*-1); penDepth = dotProduct(addSimplex, closestVector); //std::cout << closestVector << addSimplex << std::endl << std::endl; if(penDepth <= DEPTHTOLERANCE) { depth = penDepth; mtv = closestVector; return true; } if(index == simplex.size()-1) { if(simplex[simplex.size()-1] == addSimplex || simplex[0] == addSimplex) { depth = penDepth; mtv = closestVector; return true; } simplex.push_back(addSimplex); } else { std::vector<Util::Vector>::iterator it = simplex.begin(); for(int i=0; i <= index; i++) { it++; } //std::cout << *it << std::endl; if(*it == addSimplex || (*(it-1) == addSimplex)) { depth = penDepth; mtv = closestVector; return true; } simplex.insert(it, addSimplex); } } }
bool SteerLib::GJK_EPA::GJK(std::vector<Util::Vector> shape1, std::vector<Util::Vector> shape2, std::vector<Util::Vector>& simplex) { std::vector<std::vector<Util::Vector>> decomp1, decomp2; convexDecomp(decomp1, shape1); convexDecomp(decomp2, shape2); for(int i=0; i<decomp1.size(); i++) { for(int j=0; j<decomp2.size(); j++) { /*for(int k=0; k<decomp1.at(i).size(); k++) { std::cout << decomp1.at(i).at(k) << " "; } std::cout << std::endl; for(int k=0; k<decomp2.at(j).size(); k++) { std::cout << decomp2.at(j).at(k) << " "; } std::cout << std::endl;*/ Util::Vector v1, v2, v3; Util::Vector arbitraryVector = Util::Vector(1,0,0); //pick point on Minkowski difference v1 = getSupport(decomp1.at(i), arbitraryVector) - getSupport(decomp2.at(j), arbitraryVector*-1); //std::cout << getSupport(decomp1.at(i), arbitraryVector) << " " << getSupport(decomp2.at(j), arbitraryVector*-1) << std::endl; //std::cout << v1 << std::endl; //projection along v1 closest to origin arbitraryVector = v1*-1; v2 = getSupport(decomp1.at(i), arbitraryVector) - getSupport(decomp2.at(j), arbitraryVector*-1); //std::cout << v2 << std::endl; if(dotProduct(v2, arbitraryVector) <= 0) { //point added to simplex does not pass the origin, no intersect continue; } //find new vertex for simplex in direction of the origin arbitraryVector = tripleProduct(v1-v2, v2*-1, v1-v2); //std::cout << "new normal " << arbitraryVector << std::endl; if(arbitraryVector == Util::Vector(0,0,0)) { //these points are aligned with the origin float tempDot = dotProduct(v2-v1, v1*-1); float squaredLength = (v2.x-v1.x)*(v2.x-v1.x) + (v2.z-v1.z)*(v2.z-v1.z); if(tempDot < 0 || tempDot > squaredLength) //origin is not between the points, no intersect continue; } v3 = getSupport(decomp1.at(i), arbitraryVector) - getSupport(decomp2.at(j), arbitraryVector*-1); //std::cout << v3 << std::endl; while(true) { //CONDITION? //if(fabsf(v2.length() - v3.length()) <= EQUIVALENCERANGE || v1.length() - v3.length() <= EQUIVALENCERANGE) { if(v2 == v3 || v1 == v3) { //if the vertex closest to the origin is already in the simplex, no intersect break; } //std::cout << dotProduct(v3, arbitraryVector) << " " << " " << std::endl; if(dotProduct(v3, arbitraryVector) <= 0) //new point for simplex does not pass the origin, no intersect break; /*float findOrigin1, findOrigin2; //used to determine what partition of the extended edge minkowski difference that the origin is in findOrigin1 = dotProduct(v1-v3, v3*-1); std::cout << v1-v3 << std::endl; findOrigin2 = dotProduct(v2-v3, v3*-1); if(findOrigin1<0 && findOrigin2<0) //origin is outside simplex, no intersect break; else if(findOrigin1>=0 && findOrigin2>=0) { //origin is in simplex, shapes intersect std::vector<Util::Vector> returnSimplex; returnSimplex.push_back(v1); returnSimplex.push_back(v2); returnSimplex.push_back(v3); simplex = returnSimplex; std::cout << v1 << v2 << v3 << std::endl; for(int k=0; k<decomp1.at(i).size(); k++) { std::cout << decomp1.at(i).at(k) << " "; } std::cout << std::endl; for(int k=0; k<decomp2.at(j).size(); k++) { std::cout << decomp2.at(j).at(k) << " "; } return true; } if(findOrigin1>=0 && findOrigin2<0) { //keep v1, discard v2 v2 = v3; } else if(findOrigin1<0 && findOrigin2>=0) { //keep v2, discard v1 v1 = v2; v2 = v3; }*/ Util::Vector edge1, edge2; edge1 = v1-v3; edge2 = v2-v3; Util::Vector edge1Perp, edge2Perp; edge1Perp = Util::Vector(edge1.z*-1, 0, edge1.x); if(edge1Perp * edge2 > 0) edge1Perp = Util::Vector(edge1.z, 0, edge1.x*-1); edge2Perp = Util::Vector(edge2.z*-1, 0, edge2.x); if(edge2Perp * edge1 > 0) edge2Perp = Util::Vector(edge2.z, 0, edge2.x*-1); Util::Vector findOrigin1, findOrigin2; //std::cout << v1<<v2<<v3<< std::endl; findOrigin1 = edge1Perp; findOrigin2 = edge2Perp; /*findOrigin1 = tripleProduct(v1-v3, v2-v3, v2-v3); findOrigin2 = tripleProduct(v2-v3, v1-v3, v1-v3);*/ //std::cout << findOrigin1 << " " << findOrigin2 << std::endl; if(findOrigin1*(v3*-1)<=0 && findOrigin2*(v3*-1)<=0) { //simplex contains origin, intersects std::vector<Util::Vector> returnSimplex; returnSimplex.push_back(v1); returnSimplex.push_back(v2); returnSimplex.push_back(v3); simplex = returnSimplex; std::cout << v1 << v2 << v3 << std::endl; /*for(int k=0; k<decomp1.at(i).size(); k++) { std::cout << decomp1.at(i).at(k) << " "; } std::cout << std::endl; for(int k=0; k<decomp2.at(j).size(); k++) { std::cout << decomp2.at(j).at(k) << " "; }*/ return true; } else if(findOrigin1*(v3*-1)>0 && findOrigin2*(v3*-1)>0) { break; } else if(findOrigin1*(v3*-1)>0) { v2 = v3; } else { v1 = v2; v2 = v3; } arbitraryVector = tripleProduct(v1-v2, v2*-1, v1-v2); v3 = getSupport(decomp1.at(i), arbitraryVector) - getSupport(decomp2.at(j), arbitraryVector*-1); //std::cout << v1 << v2 << v3 << std::endl; } } } return false; }