protected override unsafe void OnUpdate()
{
CollisionWorld collisionWorld = buildPhysicsWorld.PhysicsWorld.CollisionWorld;
CollisionFilter collisionFilter = new CollisionFilter()
{
BelongsTo = 1u << 0,
CollidesWith = 0u | (1u << 0) | (1u << 1),
GroupIndex = 0
};
Dependency = Entities.WithAll <AnimalTag>().ForEach((ref Translation translation, ref AnimalMovementData mvmtData, in PhysicsCollider collider, in Rotation rotation) => {
Unity.Collections.NativeList <DistanceHit> hits = new Unity.Collections.NativeList <DistanceHit>(Unity.Collections.Allocator.Temp);
float maxDist = 1f;
float3 fwd = translation.Value + (mvmtData.targetDirection * 0.25f);
// Tested in Unity 2020.1.0b10
//ColliderDistanceInput cdi = new ColliderDistanceInput()
//{
// Collider = collider.ColliderPtr,
// MaxDistance = maxDist,
// Transform = new RigidTransform(rotation.Value, translation.Value)
//};
PointDistanceInput pdi = new PointDistanceInput()
{
Filter = collisionFilter,
MaxDistance = maxDist,
Position = fwd
};
if (collisionWorld.CalculateDistance(pdi, ref hits))
{
DistanceHit closest = GetClosestHit(hits, maxDist);
if (closest.Distance > 0.00001)
{
float distance = closest.Distance;
float distanceFrac = 1 - (distance / maxDist);
distanceFrac = math.clamp(distanceFrac, 0.25f, 1f);
byte colliderTags = collisionWorld.Bodies[closest.RigidBodyIndex].CustomTags;
float dotProduct = 0f;
float angle = 0f;
bool avoid = false;
if (colliderTags == 1) // Collision with another animal
{
quaternion r = collisionWorld.Bodies[closest.RigidBodyIndex].WorldFromBody.rot;
float3 othersFwd = math.rotate(r, new float3(0f, 0f, 1f));
dotProduct = (float)(math.dot(mvmtData.targetDirection, math.normalize(othersFwd)));
if (dotProduct < -0.7f)
{
avoid = true;
angle = 5.846853f * distanceFrac; //- 25f degrees
}
else if (dotProduct >= -0.001f && dotProduct < 0.975f)
{
avoid = true;
angle = (0.436332f - math.acos(dotProduct)) * distanceFrac; // 25 degrees
}
}
else if (colliderTags == 2) // Collision with terrain
{
dotProduct = math.dot(mvmtData.targetDirection, math.normalize(closest.SurfaceNormal));
if (dotProduct < -0.1f)
{
avoid = true;
angle = -(1.570796f - math.acos(dotProduct)) * distanceFrac; //90 degrees -> parallel to surface normal
}
}
if (avoid)
{
quaternion newRotation = quaternion.RotateY(angle);
float3 newDirection = math.rotate(newRotation, mvmtData.direction);
newDirection = math.normalizesafe(newDirection);
mvmtData.targetDirection = newDirection;
}
}
}
hits.Dispose();
}).Schedule(Dependency);