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;
}
