Пример #1
0
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;
        }
    }
}
Пример #2
0
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();
}
Пример #3
0
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 );
}
Пример #4
0
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;
} 
Пример #5
0
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;
}
Пример #6
0
//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;
}
Пример #7
0
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) );
	}
}
Пример #8
0
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;
}
Пример #9
0
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;
}
Пример #10
0
//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);

}
Пример #12
0
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);
		}
	}
}