private void Foreach(int key, QuadrantData me, ref float3 avoidanceForce, ref float3 convinientForce, ref int bros, float radius)
{
if (targetMap.TryGetFirstValue(key, out EntitiesHashMap.MyData other, out NativeMultiHashMapIterator <int> iterator))
{
do
{
var direction = me.position - other.position;
var distance = math.length(direction);
if (me.broId == other.data2.broId && distance < 2 * radius)
{
convinientForce += other.data2.direction;
bros++;
distance *= 2f;
}
var distanceNormalized = (radius - distance) / (radius);
if (distanceNormalized > 0f && distanceNormalized < 1f)
{
var dot = (math.dot(math.normalizesafe(-direction), math.normalizesafe(me.direction)) + 1f) * 0.5f;
var forceMultiplyer = math.length(other.data2.direction) + 0.7f;
var multiplyer = distanceNormalized * dot * forceMultiplyer;
var multiplyerSin = math.sin(multiplyer * math.PI / 2f);
avoidanceForce += math.normalizesafe(other.data2.direction) * multiplyerSin;
avoidanceForce += direction / radius;
}
} while (targetMap.TryGetNextValue(out other, ref iterator));
}
}
public void Execute(Entity e, int jobIndex, [ReadOnly] ref Translation translation, [ReadOnly] ref QuadrantEntityComponent quadEntity)
{
int hashKey = GetPositionHashMapKey(translation.Value);
QuadrantData data = new QuadrantData
{
e = e,
position = translation.Value,
quadEntityData = quadEntity
};
EntityHashMap.Add(hashKey, data);
}
private void ForeachAround(QuadrantData me, ref float3 avoidanceForce, float radius)
{
var position = me.position;
var key = QuadrantVariables.GetPositionHashMapKey(position);
Foreach(key, me, ref avoidanceForce, radius);
key = QuadrantVariables.GetPositionHashMapKey(position, new float3(1, 0, 0));
Foreach(key, me, ref avoidanceForce, radius);
key = QuadrantVariables.GetPositionHashMapKey(position, new float3(-1, 0, 0));
Foreach(key, me, ref avoidanceForce, radius);
key = QuadrantVariables.GetPositionHashMapKey(position, new float3(0, 0, 1));
Foreach(key, me, ref avoidanceForce, radius);
key = QuadrantVariables.GetPositionHashMapKey(position, new float3(0, 0, -1));
Foreach(key, me, ref avoidanceForce, radius);
}
private void Foreach(int key, QuadrantData me, ref float3 avoidanceForce, float radius)
{
if (targetMap.TryGetFirstValue(key, out EntitiesHashMap.MyData other, out NativeMultiHashMapIterator <int> iterator))
{
do
{
var direction = me.position - other.position;
var distance = math.length(direction);
var distanceNormalized = (radius - distance) / (radius);
if (distanceNormalized > 0f && distanceNormalized < 1f)
{
var multiplyer = math.dot(math.normalizesafe(me.direction), distanceNormalized) + 1f;
avoidanceForce += direction / radius;
}
} while (targetMap.TryGetNextValue(out other, ref iterator));
}
}
protected override void OnUpdate()
{
var flockingSettings = flockingSettingsSystem.FlockingSettings;
var quadrantHashMap = PathQuadrantSystem.QuadrantHashMap;
var isDebugging = IsDebugging;
Entities
.WithNone <PathProblem>()
.WithReadOnly(quadrantHashMap)
.ForEach((Entity entity, ref PathSteering steering, in PathFlocking flocking, in PathAgent agent, in LocalToWorld localToWorld) =>
{
var entityHashMapKey = PathQuadrantSystem.HashPosition(localToWorld.Position, flockingSettings);
var separationNeighbors = 0;
var alignmentNeighbors = 0;
var cohesionNeighbors = 0;
var cohesionPos = float3.zero;
var alignmentVec = float3.zero;
var separationVec = float3.zero;
var closestQuadrantData = new QuadrantData();
PathQuadrantSystem.SearchQuadrantNeighbors(
in quadrantHashMap, entityHashMapKey, entity, flocking, localToWorld.Position, flockingSettings,
ref separationNeighbors, ref alignmentNeighbors, ref cohesionNeighbors, ref cohesionPos,
ref alignmentVec, ref separationVec, ref closestQuadrantData);
var closestDistance = math.distancesq(localToWorld.Position, closestQuadrantData.LocalToWorld.Position);
if (separationNeighbors > 0)
{
separationVec /= separationNeighbors;
if (math.lengthsq(separationVec) > 0.5f)
{
separationVec += steering.CurrentHeading; // Limit clumping of close agents.
}
}
else
{
separationVec = steering.CurrentHeading;
}
if (alignmentNeighbors > 0)
{
alignmentVec /= alignmentNeighbors;
}
else
{
alignmentVec = steering.CurrentHeading;
}
if (cohesionNeighbors > 0)
{
cohesionPos /= cohesionNeighbors;
}
else
{
cohesionPos = localToWorld.Position;
}
var nearestAgentCollisionDistanceFromRadius = closestDistance - flocking.AgentAversionDistance;
var nearestAgentDirection = math.normalizesafe(localToWorld.Position - closestQuadrantData.LocalToWorld.Position);
var agentAvoidanceSteering = flockingSettings.AgentCollisionAvoidanceStrength * nearestAgentDirection;
agentAvoidanceSteering =
(nearestAgentCollisionDistanceFromRadius < 0 && !agentAvoidanceSteering.Equals(float3.zero))
? agentAvoidanceSteering
: steering.CurrentHeading;
if (nearestAgentCollisionDistanceFromRadius < 0 && isDebugging)
{
Debug.DrawRay(localToWorld.Position, -nearestAgentDirection * 2f, Color.blue);
}
var alignmentSteering = math.normalizesafe(alignmentVec) * flockingSettings.AlignmentWeight;
var cohesionSteering = math.normalizesafe(cohesionPos - localToWorld.Position) * flockingSettings.CohesionWeight;
var separationSteering = math.normalizesafe(separationVec) * flockingSettings.SeparationWeight;
steering.AgentAvoidanceSteering = agentAvoidanceSteering;
steering.CohesionSteering = cohesionSteering;
steering.SeparationSteering = separationSteering;
steering.AlignmentSteering = alignmentSteering;
}
private static void ChangeClosestTarget(ref EntityWithPosition closestTarget, ref float closestTargetDistance, QuadrantData quadrantData, EntityDescription entityDesc)
{
if (closestTarget.entity == Entity.Null)
{
// No target
closestTarget = new EntityWithPosition {
entity = quadrantData.entity, position = quadrantData.position
};
closestTargetDistance = math.distancesq(entityDesc.location.Position, quadrantData.position);
}
else
{
if (math.distancesq(entityDesc.location.Position, quadrantData.position) < closestTargetDistance)
{
// This target is closer
closestTarget = new EntityWithPosition {
entity = quadrantData.entity, position = quadrantData.position
};
closestTargetDistance = math.distancesq(entityDesc.location.Position, quadrantData.position);
}
}
}
