using System; using System.Collections.Generic; using UnityEngine; namespace Unity.MLAgents.Sensors { /// /// Determines which dimensions the sensor will perform the casts in. /// public enum RayPerceptionCastType { /// /// Cast in 2 dimensions, using Physics2D.CircleCast or Physics2D.RayCast. /// Cast2D, /// /// Cast in 3 dimensions, using Physics.SphereCast or Physics.RayCast. /// Cast3D, } /// /// Contains the elements that define a ray perception sensor. /// public struct RayPerceptionInput { /// /// Length of the rays to cast. This will be scaled up or down based on the scale of the transform. /// public float RayLength; /// /// List of tags which correspond to object types agent can see. /// public IReadOnlyList DetectableTags; /// /// List of angles (in degrees) used to define the rays. /// 90 degrees is considered "forward" relative to the game object. /// public IReadOnlyList Angles; /// /// Starting height offset of ray from center of agent /// public float StartOffset; /// /// Ending height offset of ray from center of agent. /// public float EndOffset; /// /// Radius of the sphere to use for spherecasting. /// If 0 or less, rays are used instead - this may be faster, especially for complex environments. /// public float CastRadius; /// /// Transform of the GameObject. /// public Transform Transform; /// /// Whether to perform the casts in 2D or 3D. /// public RayPerceptionCastType CastType; /// /// Filtering options for the casts. /// public int LayerMask; /// /// Returns the expected number of floats in the output. /// /// public int OutputSize() { return (DetectableTags.Count + 2) * Angles.Count; } /// /// Get the cast start and end points for the given ray index/ /// /// /// A tuple of the start and end positions in world space. public (Vector3 StartPositionWorld, Vector3 EndPositionWorld) RayExtents(int rayIndex) { var angle = Angles[rayIndex]; Vector3 startPositionLocal, endPositionLocal; if (CastType == RayPerceptionCastType.Cast3D) { startPositionLocal = new Vector3(0, StartOffset, 0); endPositionLocal = PolarToCartesian3D(RayLength, angle); endPositionLocal.y += EndOffset; } else { // Vector2s here get converted to Vector3s (and back to Vector2s for casting) startPositionLocal = new Vector2(); endPositionLocal = PolarToCartesian2D(RayLength, angle); } var startPositionWorld = Transform.TransformPoint(startPositionLocal); var endPositionWorld = Transform.TransformPoint(endPositionLocal); return (StartPositionWorld : startPositionWorld, EndPositionWorld : endPositionWorld); } /// /// Converts polar coordinate to cartesian coordinate. /// static internal Vector3 PolarToCartesian3D(float radius, float angleDegrees) { var x = radius * Mathf.Cos(Mathf.Deg2Rad * angleDegrees); var z = radius * Mathf.Sin(Mathf.Deg2Rad * angleDegrees); return new Vector3(x, 0f, z); } /// /// Converts polar coordinate to cartesian coordinate. /// static internal Vector2 PolarToCartesian2D(float radius, float angleDegrees) { var x = radius * Mathf.Cos(Mathf.Deg2Rad * angleDegrees); var y = radius * Mathf.Sin(Mathf.Deg2Rad * angleDegrees); return new Vector2(x, y); } } /// /// Contains the data generated/produced from a ray perception sensor. /// public class RayPerceptionOutput { /// /// Contains the data generated from a single ray of a ray perception sensor. /// public struct RayOutput { /// /// Whether or not the ray hit anything. /// public bool HasHit; /// /// Whether or not the ray hit an object whose tag is in the input's DetectableTags list. /// public bool HitTaggedObject; /// /// The index of the hit object's tag in the DetectableTags list, or -1 if there was no hit, or the /// hit object has a different tag. /// public int HitTagIndex; /// /// Normalized distance to the hit object. /// public float HitFraction; /// /// Writes the ray output information to a subset of the float array. Each element in the rayAngles array /// determines a sublist of data to the observation. The sublist contains the observation data for a single cast. /// The list is composed of the following: /// 1. A one-hot encoding for detectable tags. For example, if DetectableTags.Length = n, the /// first n elements of the sublist will be a one-hot encoding of the detectableTag that was hit, or /// all zeroes otherwise. /// 2. The 'numDetectableTags' element of the sublist will be 1 if the ray missed everything, or 0 if it hit /// something (detectable or not). /// 3. The 'numDetectableTags+1' element of the sublist will contain the normalized distance to the object /// hit, or 1.0 if nothing was hit. /// /// /// /// Output buffer. The size must be equal to (numDetectableTags+2) * RayOutputs.Length public void ToFloatArray(int numDetectableTags, int rayIndex, float[] buffer) { var bufferOffset = (numDetectableTags + 2) * rayIndex; if (HitTaggedObject) { buffer[bufferOffset + HitTagIndex] = 1f; } buffer[bufferOffset + numDetectableTags] = HasHit ? 0f : 1f; buffer[bufferOffset + numDetectableTags + 1] = HitFraction; } } /// /// RayOutput for each ray that was cast. /// public RayOutput[] RayOutputs; } /// /// Debug information for the raycast hits. This is used by the RayPerceptionSensorComponent. /// internal class DebugDisplayInfo { public struct RayInfo { public Vector3 worldStart; public Vector3 worldEnd; public float castRadius; public RayPerceptionOutput.RayOutput rayOutput; } public void Reset() { m_Frame = Time.frameCount; } /// /// "Age" of the results in number of frames. This is used to adjust the alpha when drawing. /// public int age { get { return Time.frameCount - m_Frame; } } public RayInfo[] rayInfos; int m_Frame; } /// /// A sensor implementation that supports ray cast-based observations. /// public class RayPerceptionSensor : ISensor { float[] m_Observations; int[] m_Shape; string m_Name; RayPerceptionInput m_RayPerceptionInput; DebugDisplayInfo m_DebugDisplayInfo; internal DebugDisplayInfo debugDisplayInfo { get { return m_DebugDisplayInfo; } } /// /// Creates the RayPerceptionSensor. /// /// The name of the sensor. /// The inputs for the sensor. public RayPerceptionSensor(string name, RayPerceptionInput rayInput) { m_Name = name; m_RayPerceptionInput = rayInput; SetNumObservations(rayInput.OutputSize()); if (Application.isEditor) { m_DebugDisplayInfo = new DebugDisplayInfo(); } } void SetNumObservations(int numObservations) { m_Shape = new[] { numObservations }; m_Observations = new float[numObservations]; } internal void SetRayPerceptionInput(RayPerceptionInput rayInput) { // Note that change the number of rays or tags doesn't directly call this, // but changing them and then changing another field will. if (m_RayPerceptionInput.OutputSize() != rayInput.OutputSize()) { Debug.Log( "Changing the number of tags or rays at runtime is not " + "supported and may cause errors in training or inference." ); // Changing the shape will probably break things downstream, but we can at least // keep this consistent. SetNumObservations(rayInput.OutputSize()); } m_RayPerceptionInput = rayInput; } /// /// Computes the ray perception observations and saves them to the provided /// . /// /// Where the ray perception observations are written to. /// public int Write(ObservationWriter writer) { using (TimerStack.Instance.Scoped("RayPerceptionSensor.Perceive")) { Array.Clear(m_Observations, 0, m_Observations.Length); var numRays = m_RayPerceptionInput.Angles.Count; var numDetectableTags = m_RayPerceptionInput.DetectableTags.Count; if (m_DebugDisplayInfo != null) { // Reset the age information, and resize the buffer if needed. m_DebugDisplayInfo.Reset(); if (m_DebugDisplayInfo.rayInfos == null || m_DebugDisplayInfo.rayInfos.Length != numRays) { m_DebugDisplayInfo.rayInfos = new DebugDisplayInfo.RayInfo[numRays]; } } // For each ray, do the casting, and write the information to the observation buffer for (var rayIndex = 0; rayIndex < numRays; rayIndex++) { DebugDisplayInfo.RayInfo debugRay; var rayOutput = PerceiveSingleRay(m_RayPerceptionInput, rayIndex, out debugRay); if (m_DebugDisplayInfo != null) { m_DebugDisplayInfo.rayInfos[rayIndex] = debugRay; } rayOutput.ToFloatArray(numDetectableTags, rayIndex, m_Observations); } // Finally, add the observations to the ObservationWriter writer.AddList(m_Observations); } return m_Observations.Length; } /// public void Update() { } /// public void Reset() { } /// public int[] GetObservationShape() { return m_Shape; } /// public string GetName() { return m_Name; } /// public virtual byte[] GetCompressedObservation() { return null; } /// public virtual SensorCompressionType GetCompressionType() { return SensorCompressionType.None; } /// /// Evaluates the raycasts to be used as part of an observation of an agent. /// /// Input defining the rays that will be cast. /// Output struct containing the raycast results. public static RayPerceptionOutput Perceive(RayPerceptionInput input) { RayPerceptionOutput output = new RayPerceptionOutput(); output.RayOutputs = new RayPerceptionOutput.RayOutput[input.Angles.Count]; for (var rayIndex = 0; rayIndex < input.Angles.Count; rayIndex++) { DebugDisplayInfo.RayInfo debugRay; output.RayOutputs[rayIndex] = PerceiveSingleRay(input, rayIndex, out debugRay); } return output; } /// /// Evaluate the raycast results of a single ray from the RayPerceptionInput. /// /// /// /// /// internal static RayPerceptionOutput.RayOutput PerceiveSingleRay( RayPerceptionInput input, int rayIndex, out DebugDisplayInfo.RayInfo debugRayOut ) { var unscaledRayLength = input.RayLength; var unscaledCastRadius = input.CastRadius; var extents = input.RayExtents(rayIndex); var startPositionWorld = extents.StartPositionWorld; var endPositionWorld = extents.EndPositionWorld; var rayDirection = endPositionWorld - startPositionWorld; // If there is non-unity scale, |rayDirection| will be different from rayLength. // We want to use this transformed ray length for determining cast length, hit fraction etc. // We also it to scale up or down the sphere or circle radii var scaledRayLength = rayDirection.magnitude; // Avoid 0/0 if unscaledRayLength is 0 var scaledCastRadius = unscaledRayLength > 0 ? unscaledCastRadius * scaledRayLength / unscaledRayLength : unscaledCastRadius; // Do the cast and assign the hit information for each detectable tag. bool castHit; float hitFraction; GameObject hitObject; if (input.CastType == RayPerceptionCastType.Cast3D) { RaycastHit rayHit; if (scaledCastRadius > 0f) { castHit = Physics.SphereCast(startPositionWorld, scaledCastRadius, rayDirection, out rayHit, scaledRayLength, input.LayerMask); } else { castHit = Physics.Raycast(startPositionWorld, rayDirection, out rayHit, scaledRayLength, input.LayerMask); } // If scaledRayLength is 0, we still could have a hit with sphere casts (maybe?). // To avoid 0/0, set the fraction to 0. hitFraction = castHit ? (scaledRayLength > 0 ? rayHit.distance / scaledRayLength : 0.0f) : 1.0f; hitObject = castHit ? rayHit.collider.gameObject : null; } else { RaycastHit2D rayHit; if (scaledCastRadius > 0f) { rayHit = Physics2D.CircleCast(startPositionWorld, scaledCastRadius, rayDirection, scaledRayLength, input.LayerMask); } else { rayHit = Physics2D.Raycast(startPositionWorld, rayDirection, scaledRayLength, input.LayerMask); } castHit = rayHit; hitFraction = castHit ? rayHit.fraction : 1.0f; hitObject = castHit ? rayHit.collider.gameObject : null; } var rayOutput = new RayPerceptionOutput.RayOutput { HasHit = castHit, HitFraction = hitFraction, HitTaggedObject = false, HitTagIndex = -1 }; if (castHit) { // Find the index of the tag of the object that was hit. var numTags = input.DetectableTags?.Count ?? 0; for (var i = 0; i < numTags; i++) { var tagsEqual = false; try { var tag = input.DetectableTags[i]; if (!string.IsNullOrEmpty(tag)) { tagsEqual = hitObject.CompareTag(tag); } } catch (UnityException) { // If the tag is null, empty, or not a valid tag, just ignore it. } if (tagsEqual) { rayOutput.HitTaggedObject = true; rayOutput.HitTagIndex = i; break; } } } debugRayOut.worldStart = startPositionWorld; debugRayOut.worldEnd = endPositionWorld; debugRayOut.rayOutput = rayOutput; debugRayOut.castRadius = scaledCastRadius; return rayOutput; } } }