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 BSPBoxSplitSelector::getSplitPlane( const util::Vector<BSPPolygon*>& polys,
ProgressIndicator* progress, Vector4* splitPlane ) const
{
assert( polys.size() > 0 );
double workUnit = polys.size();
if ( progress )
progress->addProgress( workUnit );
OBBoxBuilder builder;
while ( builder.nextPass() )
{
for ( int i = 0 ; i < polys.size() ; ++i )
{
const BSPPolygon& poly = *polys[i];
for ( int k = 0 ; k < poly.vertices() ; ++k )
builder.addPoints( &poly.getVertex(k), 1 );
}
}
OBBox box = builder.box();
Vector3 center = box.translation();
Vector3 normal = box.rotation().getColumn(0);
*splitPlane = Vector4( normal.x, normal.y, normal.z, -center.dot(normal) );
return Math::abs(normal.length()-1.f) < 1e-3f && splitPlane->finite();
}
void AnimExportUtil::resampleFloatAnimation( const util::Vector<float>& frames, Interval animRange, KeyFrameContainer* anim, float maxErr )
{
int firstFrame = animRange.Start() / SGEXPORT_TICKS_PER_SAMPLE;
require( firstFrame >= 0 && firstFrame < frames.size() );
anim->insertKey( KeyFrame(TicksToSec(animRange.Start()),INTERP_TYPE,&frames[firstFrame],1) );
anim->insertKey( KeyFrame(TicksToSec(animRange.End()),INTERP_TYPE,&frames.lastElement(),1) );
resampleFloatKeys( *anim, animRange, maxErr, frames );
if ( anim->keys() == 2 && isEqualValue(anim->getKey(0),anim->getKey(1),3) )
anim->removeKey( 1 );
}
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::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;
}
//Look at the GJK_EPA.h header file for documentation and instructions
bool SteerLib::GJK_EPA::intersect(float& return_penetration_depth, Util::Vector& return_penetration_vector, const std::vector<Util::Vector>& _shapeA, const std::vector<Util::Vector>& _shapeB)
{
if (CONCAVE_POLYGONS)
return Triangulate(_shapeA, _shapeB);
std::vector<Util::Vector> _simplex;
bool colliding;
colliding = Triangulate(_shapeA, _shapeB);
if (colliding)
{
EPA(return_penetration_depth, return_penetration_vector, _simplex, _shapeA, _shapeB);
return true;
}
else
{
return_penetration_depth = 0;
return_penetration_vector.zero();
return false;
}
// To make compiler happy
return false;
}
void AnimExportUtil::addFloatAnimation( const util::Vector<float>& frames, Interval animRange, KeyFrameContainer* anim, float maxErr )
{
int firstFrame = animRange.Start() / SGEXPORT_TICKS_PER_SAMPLE;
for ( TimeValue ticks = animRange.Start() ; ticks <= animRange.End() ; ticks += SGEXPORT_TICKS_PER_SAMPLE )
{
require( firstFrame >= 0 && firstFrame < frames.size() );
anim->insertKey( KeyFrame(TicksToSec(ticks),INTERP_TYPE,&frames[firstFrame++],1) );
}
}
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 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;
}
//Look at the GJK_EPA.h header file for documentation and instructions
bool SteerLib::GJK_EPA::intersect(float& return_penetration_depth, Util::Vector& return_penetration_vector, const std::vector<Util::Vector>& _shapeA, const std::vector<Util::Vector>& _shapeB)
{
std::vector<Util::Vector> _simplex;
bool colliding;
colliding = GJK(_simplex, _shapeA, _shapeB);
if (colliding)
{
EPA(return_penetration_depth, return_penetration_vector, _simplex, _shapeA, _shapeB);
return true;
}
else
{
return_penetration_depth = 0;
return_penetration_vector.zero();
return false;
}
}
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);
}
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);
}
}
}
