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