private void TagNeighbors(BaseGameEntity baseGameEntity, float radius)
{
_taggedNeighbors.Clear();
for (int index = _actor.World.MovingEntities.Count - 1; index >= 0; index--)
{
var neighbor = _actor.World.MovingEntities[index];
if (neighbor == baseGameEntity)
{
continue;
}
//neighbor.Tagged = false;
Vector2 vectorTo = neighbor.Position - baseGameEntity.Position;
float range = radius + neighbor.Radius; //Todo:provjeritiii da li j edobro zato sto range nije dobar
var rangeMult = range * range;
var vectorLenght = vectorTo.Length();
if (vectorLenght * vectorLenght < rangeMult)
{
_taggedNeighbors.Add(neighbor);
}
// neighbor.Tagged = true;
}
}
/// <summary>
/// Draws all BaseGameEntities
/// </summary>
/// <param name="spriteBatch"></param>
public static void DrawAll(SpriteBatch spriteBatch)
{
for (int i = _baseGameEntities.Count - 1; i >= 0; i--)
{
BaseGameEntity baseGameEntity = _baseGameEntities[i];
baseGameEntity.Draw(spriteBatch);
}
}
/// <summary>
/// Updates all BaseGameEntities
/// </summary>
/// <param name="gameTime"></param>
public static void UpdateAll(GameTime gameTime)
{
for (int i = _baseGameEntities.Count - 1; i >= 0; i--)
{
BaseGameEntity baseGameEntity = _baseGameEntities[i];
baseGameEntity.Update(gameTime);
}
}
/// <summary>
/// Imports the map from specified xml document, must be created with GLEED2D tool.
/// </summary>
/// <param name="nameOfMap">Name of the map, resides in bin/ folder</param>
/// <param name="contentManager"></param>
public void ImportMap(string nameOfMap, ContentManager contentManager)
{
_map = Level.FromFile(nameOfMap, contentManager);
_wallsLayer = _map.getLayerByName(LAYER_WALLS);
_obstaclesLayer = _map.getLayerByName(LAYER_OBSTACLES);
foreach (Item item in _obstaclesLayer.Items)
{
if (item is TextureItem)
{
var textureItem = item as TextureItem;
var baseGameEntity = new BaseGameEntity();
textureItem.load(contentManager);
baseGameEntity.Texture = contentManager.Load <Texture2D>(textureItem.asset_name);
baseGameEntity.Position = textureItem.Position;
_obstacles.Add(baseGameEntity);
}
}
}
private Vector2 ObstacleAvoidance()
{
float minDetectionBoxLenght = _actor.Radius * 4f;
//the detection box length is proportional to the agent's velocity
_obstacleDetectionBoxLenght = minDetectionBoxLenght +
((_actor.Velocity / _actor.MaxSpeed).Length() * minDetectionBoxLenght);
//tag all obstacles within range of the box for processing
// m_pVehicle.World().TagObstaclesWithinViewRange(m_pVehicle, m_dDBoxLength);
TagObstacles(_actor, _obstacleDetectionBoxLenght);
//this will keep track of the closest intersecting obstacle (CIB)
BaseGameEntity closestIntersectingObstacle = null;
//this will be used to track the distance to the CIB
float distToClosestIP = 50000f;
//this will record the transformed local coordinates of the CIB
Vector2 localPosOfClosestObstacle = new Vector2();
//foreach (var baseGameEntity in BaseGameEntity.BaseGameEntities)
foreach (BaseGameEntity taggedEntity in _taggedObstacles)
{
// Vector2 localPos = PointToLocalSpace(baseGameEntity.Position, _actor.Heading, _actor.Position);
Matrix matrix = Matrix.CreateTranslation(new Vector3(_actor.Position.X, _actor.Position.Y, 0));
Matrix invert = Matrix.Invert(matrix);
Vector2 localPos = Vector2.Transform(taggedEntity.Position, invert);
// Vector2 direction = _actor.Position - baseGameEntity.Position;
//Vector2 localPos_unused = PointToLocalSpace(baseGameEntity.Position, _actor.Heading, _actor.Side,_actor.Position);
// _debugParametars.Add(PointToWorldSpace(localPos, _actor.Heading, _actor.Side, _actor.Position));
_debugParametars.Add((_obstacleDetectionBoxLenght * _actor.Heading) + _actor.Position);
_debugParametars.Add(taggedEntity.Position);
if (localPos.X >= 0)
{
//if the distance from the x axis to the object's position is less
//than its radius + half the width of the detection box then there
//is a potential intersection.
float expandedRadius = taggedEntity.Radius + _actor.Radius;
if (Math.Abs(localPos.Y) < expandedRadius)
{
//now to do a line/circle intersection test. The center of the
//circle is represented by (cX, cY). The intersection points are
//given by the formulax=cX +/-sqrt(r^2-cY^2) for y=0.
//We only need to look at the smallest positive value of x because
//that will be the closest point of intersection.
float cX = localPos.X;
float cY = localPos.Y;
//we only need to calculate the sqrt part of the above equation once
float sqrtPart = (float)Math.Sqrt((expandedRadius * expandedRadius) - (cY * cY));
float ip = cX - sqrtPart;
if (ip <= 0)
{
ip = cX + sqrtPart;
}
//test to see if this is the closest so far. If it is, keep a
//record of the obstacle and its local coordinates
if (ip < distToClosestIP)
{
distToClosestIP = ip;
closestIntersectingObstacle = taggedEntity;
localPosOfClosestObstacle = localPos;
}
}
}
}
var steeringForce = new Vector2();
if (closestIntersectingObstacle != null)
{
//the closer the agent is to an object, the stronger the steering force
//should be
float multiplier = (float)1.0 +
((_obstacleDetectionBoxLenght - localPosOfClosestObstacle.X) /
_obstacleDetectionBoxLenght);
//calculate the lateral force
steeringForce.Y = (closestIntersectingObstacle.Radius - localPosOfClosestObstacle.Y) * multiplier;
//apply a braking force proportional to the obstacle's distance from
//the vehicle.
const float brakingWeight = 0.1f;//
steeringForce.X = ((closestIntersectingObstacle.Radius - localPosOfClosestObstacle.X) * brakingWeight);
}
//finally, convert the steering vector from local to world space
// return VectorToWorldSpace(SteeringForce, m_pVehicle.Heading(), m_pVehicle.Side());
//Matrix matrix1=new Matrix();
//Matrix.CreateRotationZ()
//worldVector = Vector2.Transform(localVector, entityWorldOrient);
Vector2 steeringVector;
var angle = VectorToAngle(_actor.Heading);
angle = MathHelper.WrapAngle(angle);
Matrix matrixR = Matrix.CreateRotationZ(angle);
steeringVector = Vector2.Transform(steeringForce, matrixR);
var steeringVector1 = VectorToWorldSpace(steeringForce, _actor.Heading, _actor.Side);
_debugParametars.Add(_actor.Position + steeringVector);
return(steeringVector);
}
