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