// Checks if origin is contained in Simplex
bool SteerLib::GJK_EPA::SimplexOrigins(std::vector<Util::Vector>& _simplex, Util::Vector& d)
{
// Set A to last thing added to Simplex
Util::Vector A = _simplex[_simplex.size() - 1];
Util::Vector A0 = -A;
if (_simplex.size() == 3) {
// IT'S A TRIANGLE!!!!!
// Get points B and C from Simplex
Util::Vector B = _simplex.at(0);
Util::Vector C = _simplex.at(1);
// Get Edges
Util::Vector AB = B - A;
Util::Vector AC = C - A;
// Get Normals
Util::Vector ABPerp = TripleProduct(AC, AB, AB);
Util::Vector ACPerp = TripleProduct(AB, AC, AC);
if (DotProduct(ABPerp, A0) > 0) {
// Remove C
_simplex.erase(_simplex.begin() + 1);
// Reset direction to ABPerp
d = ABPerp;
}
else {
if (DotProduct(ACPerp, A0) > 0) {
// Remove B
_simplex.erase(_simplex.begin());
// Reset direction to ACPerp
d = ACPerp;
}
else {
// Origin is here
return true;
}
}
}
else {
// IT'S A LINE!!!!!!
// Get point B
Util::Vector B = _simplex.at(0);
// Get Edge
Util::Vector AB = B - A;
// Get Normal
Util::Vector ABPerp = TripleProduct(AB, A0, AB);
// Reset direction to ABPerp
d = ABPerp;
}
return false;
}
Edge SteerLib::GJK_EPA::findClosestEdge(std::vector<Util::Vector> polygon) {
// Returns the edge of the polygon which is closest to the origin.
Edge closest;
closest.distance = std::numeric_limits<float>::max();
for (unsigned int i = 0; i < polygon.size(); ++i) {
unsigned int j = (i + 1 == polygon.size()) ? 0 : i + 1;
Util::Vector a = polygon[i];
Util::Vector b = polygon[j];
Util::Vector e = b - a;
Util::Vector n = TripleProduct(e, a, e);
n = Util::normalize(n);
float d = DotProduct(n, a);
if (d < closest.distance) {
closest.distance = d;
closest.normal = n;
closest.index = j;
}
}
return closest;
}
void ModelData::Add_Triangle( const Vertex& v0, const Vertex& v1, const Vertex& v2 )
{
unsigned int index0;
unsigned int index1;
unsigned int index2;
if( TripleProduct( v0, v1, v2 ) > 0.0f )
{
index0 = this->Add_Vertex( v0 );
index1 = this->Add_Vertex( v1 );
index2 = this->Add_Vertex( v2 );
}
else
{
index0 = this->Add_Vertex( v0 );
index1 = this->Add_Vertex( v2 );
index2 = this->Add_Vertex( v1 );
}
Triangle tri( index0, index1, index2 );
this->triangleList.push_back( tri );
this->triangleTextureList.push_back( -1 );
}
