Util::Vector SteerLib::GJK_EPA::normalize(Util::Vector vec) { Util::Vector normalized; float length = vec.length(); normalized = *(new Util::Vector((vec.x / length), (vec.y / length), (vec.z / length))); return normalized; }
Util::Vector SteerLib::GJK_EPA::closestMinkEdge(std::vector<Util::Vector>& simplex, float& closestDist) { closestDist = FLT_MAX; Util::Vector closestEdgeNormal; for (int i = 0; i < simplex.size(); i++) { int j; float dist; if (i + 1 == simplex.size()) { j = 0; } else { j = i + 1; } // get an edge from mink diff by getting next two points Util::Vector veci = simplex.at(i); Util::Vector vecj = simplex.at(j); Util::Vector edge = vecj - veci; Util::Vector edgeVec = crossProduct3d(crossProduct3d(edge, veci), edge); // try and avoid problems with origin being too close to edge if (edgeVec.length() <= 0.000001) { edgeVec = getNormal(edge); } edgeVec = normalize(edgeVec); // get distance of the edgeNormal dist = dotProduct3d(veci, edgeVec); if (dist < closestDist) { closestDist = dist; closestEdgeNormal = edgeVec; } } return closestEdgeNormal; }
Util::Vector SocialForcesAgent::calcAgentRepulsionForce(float dt) { Util::Vector agent_repulsion_force = 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), (this)); SteerLib::AgentInterface* tmp_agent; for (std::set<SteerLib::SpatialDatabaseItemPtr>::iterator neighbor = neighbors.begin(); neighbor != neighbors.end(); neighbor++) { if ((*neighbor)->isAgent()) { tmp_agent = dynamic_cast<SteerLib::AgentInterface*>(*neighbor); } else { continue; } if ((id() != tmp_agent->id()) && (tmp_agent->computePenetration(this->position(), this->radius()) > 0.000001)) { agent_repulsion_force = agent_repulsion_force + (tmp_agent->computePenetration(this->position(), this->radius()) * _SocialForcesParams.sf_agent_body_force) * normalize(position() - tmp_agent->position()); //tangent sliding force Util::Vector tangent = cross(cross(tmp_agent->position() - position(), velocity()), tmp_agent->position() - position()); tangent = tangent / tangent.length(); agent_repulsion_force = agent_repulsion_force + (_SocialForcesParams.sf_sliding_friction_force * (tmp_agent->computePenetration(this->position(), this->radius())) * tangent * (dot(tmp_agent->velocity() - velocity(), tangent))); } } return agent_repulsion_force; }
void SocialForcesAgent::updateAI(float timeStamp, float dt, unsigned int frameNumber) { // std::cout << "_SocialForcesParams.rvo_max_speed " << _SocialForcesParams._SocialForcesParams.rvo_max_speed << std::endl; Util::AutomaticFunctionProfiler profileThisFunction( &SocialForcesGlobals::gPhaseProfilers->aiProfiler ); if (!enabled()) { return; } Util::AxisAlignedBox oldBounds(_position.x - _radius, _position.x + _radius, 0.0f, 0.0f, _position.z - _radius, _position.z + _radius); SteerLib::AgentGoalInfo goalInfo = _goalQueue.front(); Util::Vector goalDirection; if ( ! _midTermPath.empty() && (!this->hasLineOfSightTo(goalInfo.targetLocation)) ) { if (reachedCurrentWaypoint()) { this->updateMidTermPath(); } this->updateLocalTarget(); goalDirection = normalize(_currentLocalTarget - position()); } else { goalDirection = normalize(goalInfo.targetLocation - position()); } /* * Goal Force */ Util::Vector prefForce = calcGoalForce( goalDirection, dt ); /* * Repulsion Force */ Util::Vector repulsionForce = calcRepulsionForce(dt); if ( repulsionForce.x != repulsionForce.x) { std::cout << "Found some nan" << std::endl; // repulsionForce = velocity() * 0; } /* * Proximity Force */ Util::Vector proximityForce = calcProximityForce(dt); // #define _DEBUG_ 1 #ifdef _DEBUG_ std::cout << "agent" << id() << " repulsion force " << repulsionForce << std::endl; std::cout << "agent" << id() << " proximity force " << proximityForce << std::endl; std::cout << "agent" << id() << " pref force " << prefForce << std::endl; #endif // _velocity = _newVelocity; int alpha=1; if ( repulsionForce.length() > 0.0) { alpha=0; } //_velocity = (prefForce) + repulsionForce + proximityForce; // commenting out // _velocity = velocity() + repulsionForce + proximityForce; //change to Util::Vector acceleration = (prefForce + repulsionForce + proximityForce) / AGENT_MASS; _velocity = velocity() + acceleration * dt; _velocity = clamp(velocity(), _SocialForcesParams.sf_max_speed); _velocity.y=0.0f; #ifdef _DEBUG_ std::cout << "agent" << id() << " speed is " << velocity().length() << std::endl; #endif _position = position() + (velocity() * dt); // A grid database update should always be done right after the new position of the agent is calculated /* * Or when the agent is removed for example its true location will not reflect its location in the grid database. * Not only that but this error will appear random depending on how well the agent lines up with the grid database * boundaries when removed. */ // std::cout << "Updating agent" << this->id() << " at " << this->position() << std::endl; Util::AxisAlignedBox newBounds(_position.x - _radius, _position.x + _radius, 0.0f, 0.0f, _position.z - _radius, _position.z + _radius); gSpatialDatabase->updateObject( this, oldBounds, newBounds); /* if ( ( !_waypoints.empty() ) && (_waypoints.front() - position()).length() < radius()*WAYPOINT_THRESHOLD_MULTIPLIER) { _waypoints.erase(_waypoints.begin()); } */ /* * Now do the conversion from SocialForcesAgent into the SteerSuite coordinates */ // _velocity.y = 0.0f; if ((goalInfo.targetLocation - position()).length() < radius()*GOAL_THRESHOLD_MULTIPLIER || (goalInfo.goalType == GOAL_TYPE_AXIS_ALIGNED_BOX_GOAL && Util::boxOverlapsCircle2D(goalInfo.targetRegion.xmin, goalInfo.targetRegion.xmax, goalInfo.targetRegion.zmin, goalInfo.targetRegion.zmax, this->position(), this->radius()))) { _goalQueue.pop(); // std::cout << "Made it to a goal" << std::endl; if (_goalQueue.size() != 0) { // in this case, there are still more goals, so start steering to the next goal. goalDirection = _goalQueue.front().targetLocation - _position; _prefVelocity = Util::Vector(goalDirection.x, 0.0f, goalDirection.z); } else { // in this case, there are no more goals, so disable the agent and remove it from the spatial database. disable(); return; } } // Hear the 2D solution from RVO is converted into the 3D used by SteerSuite // _velocity = Vector(velocity().x, 0.0f, velocity().z); if ( velocity().lengthSquared() > 0.0 ) { // Only assign forward direction if agent is moving // Otherwise keep last forward _forward = normalize(_velocity); } // _position = _position + (_velocity * dt); }