Util::Vector SocialForcesAgent::calcSlidingForce(float dt) {
Util::Vector returnVector = Util::Vector(0,0,0);
std::set<SteerLib::SpatialDatabaseItemPtr> _neighbors;
gSpatialDatabase->getItemsInRange(_neighbors, _position.x-(this->_radius + _SocialForcesParams.sf_query_radius), _position.x+(this->_radius + _SocialForcesParams.sf_query_radius),
_position.z-(this->_radius + _SocialForcesParams.sf_query_radius), _position.z+(this->_radius + _SocialForcesParams.sf_query_radius), dynamic_cast<SteerLib::SpatialDatabaseItemPtr>(this));
SteerLib::AgentInterface * tmp_agent;
SteerLib::ObstacleInterface * tmp_ob;
Util::Vector agFric = Util::Vector(0,0,0);
Util::Vector obsFric = Util::Vector(0,0,0);
for(std::set<SteerLib::SpatialDatabaseItemPtr>::iterator neighbor = _neighbors.begin(); neighbor != _neighbors.end(); neighbor++) {
if((*neighbor)->isAgent()) {
tmp_agent = dynamic_cast<SteerLib::AgentInterface *>(*neighbor);
if(tmp_agent->computePenetration(this->position(), this->radius()) > 0.000001) {
Util::Vector testTangent = crossProduct((this->position() - tmp_agent->position()), Util::Vector(0,1,0));
float tanVelDiff = abs(tmp_agent->velocity() * testTangent - this->velocity() * testTangent);
if((this->velocity() + tmp_agent->velocity()) * testTangent > 0) {
testTangent = testTangent * -1;
}
testTangent = testTangent / testTangent.norm();
agFric = agFric + (tmp_agent->computePenetration(this->position(), this->radius()) * _SocialForcesParams.sf_sliding_friction_force * tanVelDiff * testTangent);
}
}
else {
tmp_ob = dynamic_cast<SteerLib::ObstacleInterface *>(*neighbor);
if (tmp_ob->computePenetration(this->position(), this->radius()) > 0.000001) {
Util::Vector wall_normal = calcWallNormal(tmp_ob);
Util::Vector testTangent = crossProduct(wall_normal, Util::Vector(0,1,0));
if(this->velocity() * testTangent > 0) {
testTangent = testTangent * -1;
}
testTangent = testTangent / testTangent.norm();
float tanVelocity = abs(this->velocity() * testTangent);
std::pair<Util::Point, Util::Point> line = calcWallPointsFromNormal(tmp_ob, wall_normal);
std::pair<float, Util::Point> min_stuff = minimum_distance(line.first, line.second, position());
obsFric = obsFric + testTangent * (min_stuff.first + this->radius()) * _SocialForcesParams.sf_sliding_friction_force * tanVelocity;
}
}
}
returnVector = agFric + obsFric;
return returnVector;
}
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;
}
}
}
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);
}
}
}