public void Execute(int i)
{
AgentBehaviors mask = AgentBehaviors.Queueing;
if ((behaviors[i] & mask) == 0)
{
return;
}
AgentKinematics agent = agents[i];
float3 direction = math.normalizesafe(agent.velocity, float3.zero);
float3 ahead = agent.position + direction * maxQueueAhead;
float3 min = ahead + new float3(-maxQueueRadius, -maxQueueRadius, -maxQueueRadius);
float3 max = ahead + new float3(maxQueueRadius, maxQueueRadius, maxQueueRadius);
int3 minBucket = GetSpatialBucket(min);
int3 maxBucket = GetSpatialBucket(max);
bool obstructed;
if (minBucket.Equals(maxBucket))
{
obstructed = DetectCollision(i, ahead, minBucket);
}
else
{
obstructed = DetectCollision(i, ahead, minBucket, maxBucket);
}
if (obstructed)
{
ApplyBrakeForce(i);
}
}
public void Execute(int i)
{
AgentBehaviors mask = AgentBehaviors.Active;
if ((behaviors[i] & mask) == 0)
{
return;
}
AgentKinematics agent = agents[i];
float3 min = agent.position + new float3(-collision.radius, -collision.radius, -collision.radius);
float3 max = agent.position + new float3(collision.radius, collision.radius, collision.radius);
int3 minBucket = GetSpatialBucket(min);
int3 maxBucket = GetSpatialBucket(max);
if (minBucket.Equals(maxBucket))
{
ResolveCollisions(i, minBucket);
}
else
{
ResolveCollisions(i, minBucket, maxBucket);
}
}
public void ApplyBrakeForceToAgents()
{
NativeArray <AgentKinematics> agents = new NativeArray <AgentKinematics>(1, Allocator.Persistent);
agents[0] = new AgentKinematics()
{
velocity = new float3(0, 1, 0)
};
NativeArray <float3> steering = new NativeArray <float3>(1, Allocator.Persistent);
steering[0] = new float3(1, 0, 0);
float maxBrakeForce = 0.8f;
QueueBehavior job = new QueueBehavior()
{
maxBrakeForce = maxBrakeForce,
steering = steering,
agents = agents
};
float3 expectedVelocity = agents[0].velocity;
float3 expectedSteering = steering[0];
expectedSteering += -expectedSteering * maxBrakeForce;
expectedSteering += -agents[0].velocity;
job.ApplyBrakeForce(0);
Assert.AreEqual(expectedVelocity, agents[0].velocity); // velocity should not have changed
Assert.AreEqual(expectedSteering, steering[0]); // steering should have changed
steering.Dispose();
agents.Dispose();
}
public void Execute(int i)
{
AgentBehaviors mask = AgentBehaviors.Active;
if ((behaviors[i] & mask) == 0)
{
return;
}
AgentKinematics agent = agents[i];
int3 grid = GridPosition(agent.position);
grid += new int3(0, -1, 0);
byte voxel = voxels[Flatten(grid)];
if (OverAir(voxel))
{
steering[i] += gravity;
}
if (InSolid(voxel))
{
steering[i] += gravity * -0.5f;
}
}
public void AgentsRiseWhenBuried()
{
NativeArray <AgentKinematics> agents = new NativeArray <AgentKinematics>(1, Allocator.Persistent);
agents[0] = new AgentKinematics()
{
velocity = new float3(0, 0, 0)
};
NativeArray <float3> steering = new NativeArray <float3>(1, Allocator.Persistent);
steering[0] = new float3(0, 0, 0);
NativeArray <AgentBehaviors> active = new NativeArray <AgentBehaviors>(1, Allocator.Persistent);
active[0] = AgentBehaviors.Active;
NativeArray <VRoxel.Navigation.Block> blocks = new NativeArray <VRoxel.Navigation.Block>(1, Allocator.Persistent);
VRoxel.Navigation.Block solidBlock = new VRoxel.Navigation.Block();
solidBlock.solid = true; solidBlock.cost = 1; blocks[0] = solidBlock;
NativeArray <byte> voxels = new NativeArray <byte>(1, Allocator.Persistent);
AgentWorld world = new AgentWorld()
{
scale = 1f,
offset = float3.zero,
center = new float3(0.5f, 0.5f, 0.5f),
rotation = quaternion.identity,
};
GravityBehavior job = new GravityBehavior()
{
gravity = new float3(0, -4f, 0),
world = world,
steering = steering,
behaviors = active,
agents = agents,
blocks = blocks,
voxels = voxels,
};
job.Schedule(1, 1).Complete();
Assert.AreEqual(new float3(0, 2, 0), steering[0]);
agents.Dispose();
active.Dispose();
blocks.Dispose();
voxels.Dispose();
steering.Dispose();
}
public void CanCheckForCollisions()
{
int height = 1;
float radius = 1f;
float3 start = new float3(0.25f, 0, 0);
AgentKinematics self = new AgentKinematics()
{
position = float3.zero
};
SpatialMapData target = new SpatialMapData()
{
position = start, height = height
};
CollisionBehavior job = new CollisionBehavior()
{
minDistance = 0.01f,
collision = new AgentCollision()
{
radius = radius, height = height
}
};
/// test 2D circle intersection
bool result;
result = job.Collision(self, target);
Assert.AreEqual(true, result); // inside
target.position += new float3(0.25f, 0, 0);
result = job.Collision(self, target);
Assert.AreEqual(true, result); // on the edge
target.position += new float3(1f, 0, 0);
result = job.Collision(self, target);
Assert.AreEqual(false, result); // outside
/// test height intersection
target.position = new float3(0, 0.5f, 0);
result = job.Collision(self, target);
Assert.AreEqual(true, result); // inside (collision)
target.position = new float3(0, 1f, 0);
result = job.Collision(self, target);
Assert.AreEqual(false, result); // above (no collision)
target.position = new float3(0, -1f, 0);
result = job.Collision(self, target);
Assert.AreEqual(false, result); // below (no collision)
}
/// <summary>
/// Changes the current max speed of an agent
/// </summary>
public void SetMaxSpeed(NavAgent agent, float maxSpeed)
{
movingAllAgents.Complete();
int archetypeIndex = archetypes.IndexOf(agent.configuration.archetype);
AgentKinematics kinematics = m_agentKinematics[archetypeIndex][agent.index];
kinematics.maxSpeed = maxSpeed;
NativeArray <AgentKinematics> agentKinematics = m_agentKinematics[archetypeIndex];
agentKinematics[agent.index] = kinematics;
}
public void DetectCollisionsWithAgents()
{
float3 position1 = float3.zero;
float3 position2 = new float3(0, 0.5f, 0);
NativeMultiHashMap <int3, SpatialMapData> spatialMap = new NativeMultiHashMap <int3, SpatialMapData>(1, Allocator.Persistent);
spatialMap.Add(int3.zero, new SpatialMapData()
{
position = position1
});
spatialMap.Add(int3.zero, new SpatialMapData()
{
position = position2
});
NativeArray <AgentKinematics> agents = new NativeArray <AgentKinematics>(2, Allocator.Persistent);
agents[0] = new AgentKinematics()
{
position = position1
};
agents[1] = new AgentKinematics()
{
position = position2
};
QueueBehavior job = new QueueBehavior()
{
spatialMap = spatialMap,
maxQueueRadius = 0.25f,
agents = agents,
maxDepth = 100,
};
bool collision;
collision = job.DetectCollision(0, float3.zero, int3.zero);
Assert.AreEqual(false, collision);
collision = job.DetectCollision(0, new float3(0, 0.25f, 0), int3.zero);
Assert.AreEqual(true, collision);
spatialMap.Dispose();
agents.Dispose();
}
public void Execute(int i)
{
AgentBehaviors mask = AgentBehaviors.Avoiding;
if ((behaviors[i] & mask) == 0)
{
return;
}
AgentKinematics agent = agents[i];
float dynamicLength = math.length(agent.velocity) / avoidDistance;
float3 direction = math.normalizesafe(agent.velocity, float3.zero);
float3 closest = new float3(float.MaxValue, float.MaxValue, float.MaxValue);
float3 ahead = agent.position + direction * dynamicLength;
float3 ahead2 = agent.position + direction * dynamicLength * 0.5f;
float3 min = ahead + new float3(-avoidRadius, -avoidRadius, -avoidRadius);
float3 max = ahead + new float3(avoidRadius, avoidRadius, avoidRadius);
float3 min2 = ahead2 + new float3(-avoidRadius, -avoidRadius, -avoidRadius);
float3 max2 = ahead2 + new float3(avoidRadius, avoidRadius, avoidRadius);
int3 minBucket = math.min(GetSpatialBucket(min), GetSpatialBucket(min2));
int3 maxBucket = math.max(GetSpatialBucket(max), GetSpatialBucket(max2));
bool shouldAvoid;
if (minBucket.Equals(maxBucket))
{
shouldAvoid = DetectObstruction(i, ahead, ahead2, minBucket, ref closest);
}
else
{
shouldAvoid = DetectObstruction(i, ahead, ahead2, minBucket, maxBucket, ref closest);
}
if (shouldAvoid)
{
ApplyAvoidanceForce(i, ahead, closest);
}
}
/// <summary>
/// Checks if there is a collision between two agents
/// using their collision radius and height
/// </summary>
public bool Collision(AgentKinematics self, SpatialMapData target)
{
// skip if the target agent is the same as the source agent
if (self.position.Equals(target.position))
{
return(false);
}
// find the distance between the two agents
// clamp y-axis to test 2D circle intersection
float3 delta = self.position - target.position;
float deltaY = delta.y;
delta.y = 0;
// use the combined radius of the two agents to
// test if their collision radius intersect
float distance = math.length(delta);
float radius = collision.radius + target.radius;
bool intersectCircle = distance <= radius;
// compare the two agent's height and offset for collisions
// use delta Y to test that the agents are colliding with each other
bool intersectHeight;
if (deltaY >= 0) // target is above the agent
{
intersectHeight = deltaY - collision.height < 0;
}
else // target is below the agent
{
intersectHeight = -deltaY - target.height < 0;
}
// agents must be intersecting the 2D circle
// and height to be considered a collision
return(intersectCircle && intersectHeight);
}
public void Execute(int i, TransformAccess transform)
{
AgentBehaviors mask = AgentBehaviors.Active;
if ((behaviors[i] & mask) == 0)
{
return;
}
float3 up = new float3(0, 1, 0);
float3 position = transform.position;
quaternion rotation = transform.rotation;
AgentMovement movement = movementTypes[agentMovement[i]];
AgentKinematics agent = agents[i];
steering[i] = Clamp(steering[i], maxForce);
steering[i] = steering[i] / movement.mass;
agent.velocity = Clamp(agent.velocity + steering[i], agent.maxSpeed);
steering[i] = float3.zero; // reset steering forces for next frame
agents[i] = agent; // update the kinematics for the next frame
float3 nextPosition = agent.position + agent.velocity * deltaTime;
int3 nextGrid = GridPosition(nextPosition);
if (OutOfBounds(nextGrid))
{
return;
}
transform.position = nextPosition;
if (agent.velocity.Equals(float3.zero))
{
return;
}
quaternion look = quaternion.LookRotation(agent.velocity, up);
transform.rotation = math.slerp(rotation, look, movement.turnSpeed * deltaTime);
}
public void Execute(int i)
{
AgentBehaviors mask = AgentBehaviors.Seeking;
if ((behaviors[i] & mask) == 0)
{
return;
}
AgentKinematics agent = agents[i];
int3 grid = GridPosition(agent.position);
grid += new int3(0, -1, 0);
if (OutOfBounds(grid))
{
steering[i] = float3.zero;
return;
}
int fieldIndex = Flatten(grid);
byte directionIndex = flowField[fieldIndex];
if (directionIndex == 0) // no direction
{
steering[i] = float3.zero;
return;
}
int3 flowDirection = flowDirections[directionIndex];
int3 nextGrid = grid + flowDirection + new int3(0, 1, 0);
float3 nextPosition = ScenePosition(nextGrid);
float3 desired = nextPosition - agent.position;
desired = math.normalizesafe(desired, float3.zero);
desired *= movementTypes[agentMovement[i]].topSpeed;
steering[i] = desired - agent.velocity;
}
//-------------------------------------------------
#region Public API
/// <summary>
/// Initializes the agent manager with an array of agent transforms
/// </summary>
public virtual void Initialize(Dictionary <NavAgentArchetype, List <Transform> > agents)
{
Dispose(); // clear any existing memory
int configCount = configurations.Count;
int archetypeCount = archetypes.Count;
// initialize agent movement configurations
m_movementTypes = new NativeArray <AgentMovement>(configCount, Allocator.Persistent);
for (int i = 0; i < configCount; i++)
{
m_movementTypes[i] = configurations[i].movement;
}
// initialize job handle arrays
m_updatingHandles = new NativeArray <JobHandle>(archetypeCount, Allocator.Persistent);
m_movingByArchetype = new NativeArray <JobHandle>(archetypeCount, Allocator.Persistent);
// initialize the agent properties
m_agentMovementTypes = new List <NativeArray <int> >(archetypeCount);
m_agentBehaviors = new List <NativeArray <AgentBehaviors> >(archetypeCount);
m_agentKinematics = new List <NativeArray <AgentKinematics> >(archetypeCount);
m_transformAccess = new List <TransformAccessArray>(archetypeCount);
m_agentSteering = new List <NativeArray <float3> >(archetypeCount);
m_totalAgents = new List <int>(archetypeCount);
// initialize the flow field data structures
int length = world.size.x * world.size.y * world.size.z;
m_flowFields = new List <NativeArray <byte> >(archetypeCount);
m_costFields = new List <NativeArray <byte> >(archetypeCount);
m_intFields = new List <NativeArray <ushort> >(archetypeCount);
m_openNodes = new List <NativeQueue <int3> >(archetypeCount);
// cached data to build the flow fields
m_targetPositions = new NativeList <int3>(Allocator.Persistent);
// initialize each archetype
for (int i = 0; i < archetypeCount; i++)
{
// initialize flow field data for each archetype
m_openNodes.Add(new NativeQueue <int3>(Allocator.Persistent));
m_flowFields.Add(new NativeArray <byte>(length, Allocator.Persistent));
m_costFields.Add(new NativeArray <byte>(length, Allocator.Persistent));
m_intFields.Add(new NativeArray <ushort>(length, Allocator.Persistent));
// initialize agent data for each archetype
Transform[] transforms = agents[archetypes[i]].ToArray();
int count = transforms.Length;
TransformAccessArray transformAccess = new TransformAccessArray(transforms);
NativeArray <int> movementTypes = new NativeArray <int>(count, Allocator.Persistent);
NativeArray <float3> steering = new NativeArray <float3>(count, Allocator.Persistent);
NativeArray <AgentKinematics> kinematics = new NativeArray <AgentKinematics>(count, Allocator.Persistent);
NativeArray <AgentBehaviors> behaviors = new NativeArray <AgentBehaviors>(count, Allocator.Persistent);
// read agent configuration
for (int a = 0; a < count; a++)
{
NavAgent agent = transforms[a].GetComponent <NavAgent>();
int movementType = configurations.IndexOf(agent.configuration);
AgentKinematics agentKinematics = new AgentKinematics();
agentKinematics.maxSpeed = agent.configuration.movement.topSpeed;
movementTypes[a] = movementType;
kinematics[a] = agentKinematics;
}
m_agentMovementTypes.Add(movementTypes);
m_transformAccess.Add(transformAccess);
m_agentKinematics.Add(kinematics);
m_agentBehaviors.Add(behaviors);
m_agentSteering.Add(steering);
m_totalAgents.Add(count);
}
// initialize the agent spatial map
m_spatialMap = new NativeMultiHashMap <int3, SpatialMapData>(m_totalAgents.Sum(), Allocator.Persistent);
m_spatialMapWriter = m_spatialMap.AsParallelWriter();
// cache all directions
m_directions = new NativeArray <int3>(27, Allocator.Persistent);
for (int i = 0; i < 27; i++)
{
Vector3Int dir = Core.Direction3Int.Directions[i];
m_directions[i] = new int3(dir.x, dir.y, dir.z);
}
// cache directions for climbable check
m_directionsNESW = new NativeArray <int>(4, Allocator.Persistent);
m_directionsNESW[0] = 3;
m_directionsNESW[1] = 5;
m_directionsNESW[2] = 7;
m_directionsNESW[3] = 9;
// convert blocks to a struct and cache the data
byte defaultCost = 1;
int blockCount = blockManager.blocks.Count;
m_blockTypes = new List <NativeArray <Block> >();
for (int a = 0; a < archetypeCount; a++)
{
NativeArray <Block> blocks = new NativeArray <Block>(
blockCount, Allocator.Persistent);
for (int i = 0; i < blockCount; i++)
{
BlockConfiguration block = blockManager.blocks[i];
int index = archetypes[a].movementCosts
.FindIndex(x => x.block == block);
Block navBlock = new Block();
if (index == -1)
{
navBlock.cost = defaultCost;
}
else
{
navBlock.cost = archetypes[a]
.movementCosts[index].cost;
}
navBlock.solid = block.collidable;
blocks[i] = navBlock;
}
m_blockTypes.Add(blocks);
}
}
public void UpdatesPositionAndRotation()
{
Transform[] transforms = new Transform[2];
transforms[0] = GameObject.CreatePrimitive(PrimitiveType.Cube).transform;
transforms[1] = GameObject.CreatePrimitive(PrimitiveType.Cube).transform;
TransformAccessArray asyncTransforms = new TransformAccessArray(transforms);
NativeArray <AgentKinematics> agents = new NativeArray <AgentKinematics>(2, Allocator.Persistent);
agents[0] = new AgentKinematics()
{
maxSpeed = 1f
};
agents[1] = new AgentKinematics()
{
maxSpeed = 1f
};
NativeArray <float3> directions = new NativeArray <float3>(2, Allocator.Persistent);
directions[0] = Vector3.right;
directions[1] = Vector3.left;
NativeArray <byte> flowField = new NativeArray <byte>(1, Allocator.Persistent);
for (int i = 0; i < 1; i++)
{
flowField[i] = 1;
}
NativeArray <AgentBehaviors> active = new NativeArray <AgentBehaviors>(2, Allocator.Persistent);
for (int i = 0; i < 2; i++)
{
active[i] = AgentBehaviors.Active;
}
NativeArray <int> agentMovementTypes = new NativeArray <int>(2, Allocator.Persistent);
NativeArray <AgentMovement> movementTypes = new NativeArray <AgentMovement>(1, Allocator.Persistent);
movementTypes[0] = new AgentMovement()
{
mass = 1f, topSpeed = 1f, turnSpeed = 1f
};
AgentWorld world = new AgentWorld()
{
scale = 1f,
offset = float3.zero,
center = new float3(0.5f, 0.5f, 0.5f),
rotation = quaternion.identity,
};
MoveAgentJob job = new MoveAgentJob()
{
maxForce = 1f,
movementTypes = movementTypes,
agentMovement = agentMovementTypes,
agents = agents,
behaviors = active,
steering = directions,
deltaTime = Time.deltaTime,
world = world,
flowField = flowField,
flowFieldSize = new int3(1, 1, 1)
};
Vector3 position_0 = transforms[0].position;
Vector3 position_1 = transforms[1].position;
Quaternion rotation_0 = transforms[0].rotation;
Quaternion rotation_1 = transforms[1].rotation;
JobHandle handle = job.Schedule(asyncTransforms);
handle.Complete();
Assert.AreNotEqual(position_0, transforms[0].position);
Assert.AreNotEqual(position_1, transforms[1].position);
Assert.AreNotEqual(rotation_0, transforms[0].rotation);
Assert.AreNotEqual(rotation_1, transforms[1].rotation);
agents.Dispose();
active.Dispose();
flowField.Dispose();
directions.Dispose();
asyncTransforms.Dispose();
agentMovementTypes.Dispose();
movementTypes.Dispose();
foreach (var t in transforms)
{
GameObject.DestroyImmediate(t.gameObject);
}
}
public void AddBrakeForceWhenAgentsCollide()
{
float3 position1 = float3.zero;
float3 position2 = new float3(0f, 1f, 0f);
NativeMultiHashMap <int3, SpatialMapData> spatialMap = new NativeMultiHashMap <int3, SpatialMapData>(1, Allocator.Persistent);
spatialMap.Add(int3.zero, new SpatialMapData()
{
position = position1
});
spatialMap.Add(int3.zero, new SpatialMapData()
{
position = position2
});
NativeArray <AgentKinematics> agents = new NativeArray <AgentKinematics>(2, Allocator.Persistent);
agents[0] = new AgentKinematics()
{
position = position1, velocity = new float3(0, 0, 0)
};
agents[1] = new AgentKinematics()
{
position = position2, velocity = new float3(0, 1, 0)
};
NativeArray <float3> steering = new NativeArray <float3>(2, Allocator.Persistent);
steering[0] = new float3(0, 1, 0);
steering[1] = new float3(1, 0, 1);
NativeArray <AgentBehaviors> active = new NativeArray <AgentBehaviors>(2, Allocator.Persistent);
for (int i = 0; i < 2; i++)
{
active[i] = AgentBehaviors.Queueing;
}
AgentWorld world = new AgentWorld()
{
scale = 1f,
offset = float3.zero,
center = new float3(0.5f, 0.5f, 0.5f),
rotation = quaternion.identity,
};
QueueBehavior job = new QueueBehavior()
{
maxBrakeForce = 0.8f,
maxQueueAhead = 1f,
maxQueueRadius = 1f,
maxDepth = 100,
world = world,
size = new int3(1, 1, 1),
spatialMap = spatialMap,
steering = steering,
behaviors = active,
agents = agents
};
job.Schedule(2, 1).Complete();
// first agent should be braking
float3 expected = new float3(0, 0.2f, 0);
Assert.AreEqual(true, Mathf.Approximately(expected.x, steering[0].x));
Assert.AreEqual(true, Mathf.Approximately(expected.y, steering[0].y));
Assert.AreEqual(true, Mathf.Approximately(expected.z, steering[0].z));
// second agent should not be braking
expected = new float3(1, 0, 1);
Assert.AreEqual(true, Mathf.Approximately(expected.x, steering[1].x));
Assert.AreEqual(true, Mathf.Approximately(expected.y, steering[1].y));
Assert.AreEqual(true, Mathf.Approximately(expected.z, steering[1].z));
spatialMap.Dispose();
steering.Dispose();
agents.Dispose();
active.Dispose();
}
public void UpdatesDirections()
{
NativeArray <float3> directions = new NativeArray <float3>(1, Allocator.Persistent);
NativeArray <AgentKinematics> agents = new NativeArray <AgentKinematics>(1, Allocator.Persistent);
agents[0] = new AgentKinematics()
{
position = Vector3.up
};
NativeArray <AgentBehaviors> active = new NativeArray <AgentBehaviors>(1, Allocator.Persistent);
active[0] = AgentBehaviors.Seeking;
NativeArray <byte> flowField = new NativeArray <byte>(1, Allocator.Persistent);
NativeArray <int3> flowDirections = new NativeArray <int3>(27, Allocator.Persistent);
for (int i = 0; i < 1; i++)
{
flowField[i] = (byte)Direction3Int.Name.Up;
}
for (int i = 0; i < 27; i++)
{
Vector3Int dir = Direction3Int.Directions[i];
flowDirections[i] = new int3(dir.x, dir.y, dir.z);
}
NativeArray <int> agentMovementTypes = new NativeArray <int>(1, Allocator.Persistent);
NativeArray <AgentMovement> movementTypes = new NativeArray <AgentMovement>(1, Allocator.Persistent);
movementTypes[0] = new AgentMovement()
{
mass = 1f, topSpeed = 1f, turnSpeed = 1f
};
AgentWorld world = new AgentWorld()
{
scale = 1f,
offset = float3.zero,
center = new float3(0.5f, 0.5f, 0.5f),
rotation = quaternion.identity,
};
FlowFieldSeekJob job = new FlowFieldSeekJob()
{
movementTypes = movementTypes,
agentMovement = agentMovementTypes,
world = world,
flowField = flowField,
flowDirections = flowDirections,
flowFieldSize = new int3(1, 1, 1),
agents = agents,
steering = directions,
behaviors = active,
};
JobHandle handle = job.Schedule(1, 1);
handle.Complete();
Assert.AreEqual(new float3(0, 1, 0), directions[0]);
flowDirections.Dispose();
flowField.Dispose();
directions.Dispose();
agents.Dispose();
active.Dispose();
movementTypes.Dispose();
agentMovementTypes.Dispose();
}
public void AvoidsNearbyAgents()
{
float3 position1 = float3.zero;
float3 position2 = new float3(0f, 1f, 0f);
float3 max = new float3(float.MaxValue, float.MaxValue, float.MaxValue);
NativeMultiHashMap <int3, SpatialMapData> spatialMap = new NativeMultiHashMap <int3, SpatialMapData>(1, Allocator.Persistent);
spatialMap.Add(int3.zero, new SpatialMapData()
{
position = position1
});
spatialMap.Add(int3.zero, new SpatialMapData()
{
position = position2
});
NativeArray <AgentKinematics> agents = new NativeArray <AgentKinematics>(2, Allocator.Persistent);
agents[0] = new AgentKinematics()
{
position = position1, velocity = new float3(0, 0, 0)
};
agents[1] = new AgentKinematics()
{
position = position2, velocity = new float3(0, 1, 0)
};
NativeArray <float3> steering = new NativeArray <float3>(2, Allocator.Persistent);
NativeArray <AgentBehaviors> active = new NativeArray <AgentBehaviors>(2, Allocator.Persistent);
AgentBehaviors flags = AgentBehaviors.Avoiding;
for (int i = 0; i < 2; i++)
{
active[i] = flags;
}
AgentWorld world = new AgentWorld()
{
scale = 1f,
offset = float3.zero,
center = new float3(0.5f, 0.5f, 0.5f),
rotation = quaternion.identity,
};
AvoidCollisionBehavior job = new AvoidCollisionBehavior()
{
avoidDistance = 1f,
avoidRadius = 1f,
avoidForce = 1f,
maxDepth = 100,
world = world,
size = new int3(1, 1, 1),
spatialMap = spatialMap,
steering = steering,
behaviors = active,
agents = agents,
};
job.Schedule(2, 1).Complete();
Assert.AreEqual(new float3(0, -1, 0), steering[0]);
Assert.AreEqual(new float3(0, 0, 0), steering[1]);
spatialMap.Dispose();
steering.Dispose();
agents.Dispose();
active.Dispose();
}
public void CanDetectAnObstruction()
{
float3 position1 = float3.zero;
float3 position2 = new float3(0f, 1f, 0f);
float3 max = new float3(float.MaxValue, float.MaxValue, float.MaxValue);
NativeMultiHashMap <int3, SpatialMapData> spatialMap = new NativeMultiHashMap <int3, SpatialMapData>(1, Allocator.Persistent);
spatialMap.Add(int3.zero, new SpatialMapData()
{
position = position1
});
spatialMap.Add(int3.zero, new SpatialMapData()
{
position = position2
});
NativeArray <AgentKinematics> agents = new NativeArray <AgentKinematics>(2, Allocator.Persistent);
agents[0] = new AgentKinematics()
{
position = position1
};
agents[1] = new AgentKinematics()
{
position = position2
};
AvoidCollisionBehavior job = new AvoidCollisionBehavior()
{
maxDepth = 100,
avoidRadius = 0.5f,
spatialMap = spatialMap,
agents = agents,
};
// when ahead vector is in range
bool obstructed;
float3 ahead = new float3(0, 1f, 0);
float3 ahead2 = new float3(0, 0.5f, 0);
float3 closest = max;
obstructed = job.DetectObstruction(0, ahead, ahead2, int3.zero, ref closest);
Assert.AreEqual(true, obstructed);
Assert.AreEqual(position2, closest);
// when ahead2 vector is in range
ahead = new float3(0, 2, 0);
ahead2 = new float3(0, 1, 0);
closest = max;
obstructed = job.DetectObstruction(0, ahead, ahead2, int3.zero, ref closest);
Assert.AreEqual(true, obstructed);
Assert.AreEqual(position2, closest);
// when not obstructed
ahead = new float3(0, 0.25f, 0);
ahead2 = new float3(0, 0.125f, 0);
closest = max;
obstructed = job.DetectObstruction(0, ahead, ahead2, int3.zero, ref closest);
Assert.AreEqual(false, obstructed);
Assert.AreEqual(max, closest);
agents.Dispose();
spatialMap.Dispose();
}
