minkowskiDifference_t buildMinkowskiDifference(std::vector<MathVector> a, std::vector<MathVector> b) { MathVector direction = MathVector(1,1); std::vector<MathVector> simplex; simplex.push_back(getSupportVertex(a, b, direction)); minkowskiDifference_t difference; direction = direction.negate(); while(true) { simplex.push_back(getSupportVertex(a, b, direction)); if(simplex.back().dotProduct(direction) <= 0) { difference.colliding = false; difference.collisionNormal = MathVector(0,0); difference.collisionDepth = 0; return difference; } else if(containsOrigin(simplex, direction) && simplex.size() == 3) { while(true) { //Perform EPA to get collision normal and penetration distance Edge_t e = findClosestEdge(simplex); MathVector p = getSupportVertex(a, b, direction); double d = p.dotProduct(e.normal); // std::cout << d - e.distance << std::endl; // std::cout << "Simplex size: " << simplex.size() << std::endl; if(d - e.distance < TOLERANCE) { difference.collisionNormal = e.normal; difference.collisionDepth = d; difference.colliding = true; return difference; } else { // std::cout << "Closest edge not found in this iteration, adding point to simplex and continuing." << std::endl; simplex.insert((simplex.begin()+e.index),p); } } } } }
MathVector Gui::getSize() { return MathVector(); }
MathVector MathVector::getNormalized() { double length = getLength(); return MathVector(x / length, y / length, z / length); }
MathVector MathVector::operator-(MathVector v) { return MathVector(x - v.x, y - v.y, z - v.z); }
MathVector MathVector::operator+(MathVector v) { return MathVector(x + v.x, y + v.y, z + v.z); }
MathVector MathVector::operator/(double scalar) { return MathVector(x / scalar, y / scalar, z / scalar); }
MathVector MathVector::operator*(double scalar) { return MathVector(x * scalar, y * scalar, z * scalar); }