// Copyright (c) Microsoft Corporation. // Licensed under the MIT License. using Microsoft.MixedReality.Toolkit.Physics; using System; using UnityEngine; using UnityEngine.Serialization; namespace Microsoft.MixedReality.Toolkit.Utilities.Solvers { /// /// SurfaceMagnetism casts rays to Surfaces in the world and aligns the object to the hit surface. /// [AddComponentMenu("Scripts/MRTK/SDK/SurfaceMagnetism")] public class SurfaceMagnetism : Solver { #region Enums /// /// Raycast direction mode for solver /// public enum RaycastDirectionMode { /// /// Cast from Tracked Target in facing direction /// TrackedTargetForward = 0, /// /// Cast from Tracked Target position to this object's position /// ToObject, /// /// Cast from Tracked Target Position to linked solver position /// ToLinkedPosition, } /// /// Orientation mode for solver /// public enum OrientationMode { /// /// No orienting /// None = 0, /// /// Face the tracked transform /// TrackedTarget = 1, /// /// Aligned to surface normal completely /// SurfaceNormal = 2, /// /// Blend between tracked transform and the surface normal orientation /// Blended = 3, /// /// Face toward this object's position /// TrackedOrigin = 4, } #endregion #region SurfaceMagnetism Parameters [SerializeField] [Tooltip("Array of LayerMask to execute from highest to lowest priority. First layermask to provide a raycast hit will be used by component")] private LayerMask[] magneticSurfaces = { UnityEngine.Physics.DefaultRaycastLayers }; /// /// Array of LayerMask to execute from highest to lowest priority. First layermask to provide a raycast hit will be used by component /// public LayerMask[] MagneticSurfaces { get => magneticSurfaces; set => magneticSurfaces = value; } [SerializeField] [Tooltip("Max distance for raycast to check for surfaces")] [FormerlySerializedAs("maxDistance")] private float maxRaycastDistance = 50.0f; /// /// Max distance for raycast to check for surfaces /// public float MaxRaycastDistance { get => maxRaycastDistance; set => maxRaycastDistance = value; } [SerializeField] [Tooltip("Closest distance to bring object")] [FormerlySerializedAs("closeDistance")] private float closestDistance = 0.5f; /// /// Closest distance to bring object /// public float ClosestDistance { get => closestDistance; set => closestDistance = value; } [SerializeField] [Tooltip("Offset from surface along surface normal")] private float surfaceNormalOffset = 0.5f; /// /// Offset from surface along surface normal /// public float SurfaceNormalOffset { get => surfaceNormalOffset; set => surfaceNormalOffset = value; } [SerializeField] [Tooltip("Offset from surface along ray cast direction")] private float surfaceRayOffset = 0; /// /// Offset from surface along ray cast direction /// public float SurfaceRayOffset { get => surfaceRayOffset; set => surfaceRayOffset = value; } [SerializeField] [Tooltip("Surface raycast mode for solver")] private SceneQueryType raycastMode = SceneQueryType.SimpleRaycast; /// /// Surface raycast mode for solver /// public SceneQueryType RaycastMode { get => raycastMode; set => raycastMode = value; } #region Box Raycast Parameters [SerializeField] [Tooltip("Number of rays per edge, should be odd. Total casts is n^2")] private int boxRaysPerEdge = 3; /// /// Number of rays per edge, should be odd. Total casts is n^2 /// public int BoxRaysPerEdge { get => boxRaysPerEdge; set => boxRaysPerEdge = value; } [SerializeField] [Tooltip("If true, use orthographic casting for box lines instead of perspective")] private bool orthographicBoxCast = false; /// /// If true, use orthographic casting for box lines instead of perspective /// public bool OrthographicBoxCast { get => orthographicBoxCast; set => orthographicBoxCast = value; } [SerializeField] [Tooltip("Align to ray cast direction if box cast hits many normals facing in varying directions")] private float maximumNormalVariance = 0.5f; /// /// Align to ray cast direction if box cast hits many normals facing in varying directions /// public float MaximumNormalVariance { get => maximumNormalVariance; set => maximumNormalVariance = value; } #endregion #region Sphere Raycast Parameters [SerializeField] [Tooltip("Radius to use for sphere cast")] private float sphereSize = 1.0f; /// /// Radius to use for sphere cast /// public float SphereSize { get => sphereSize; set => sphereSize = value; } #endregion [SerializeField] [Tooltip("When doing volume casts, use size override if non-zero instead of object's current scale")] private float volumeCastSizeOverride = 0; /// /// When doing volume casts, use size override if non-zero instead of object's current scale /// public float VolumeCastSizeOverride { get => volumeCastSizeOverride; set => volumeCastSizeOverride = value; } [SerializeField] [Tooltip("When doing volume casts, use linked AltScale instead of object's current scale")] private bool useLinkedAltScaleOverride = false; /// /// When doing volume casts, use linked AltScale instead of object's current scale /// public bool UseLinkedAltScaleOverride { get => useLinkedAltScaleOverride; set => useLinkedAltScaleOverride = value; } [SerializeField] [Tooltip("Raycast direction type. Default is forward direction of Tracked Target transform")] private RaycastDirectionMode currentRaycastDirectionMode = RaycastDirectionMode.TrackedTargetForward; /// /// Raycast direction type. Default is forward direction of Tracked Target transform /// public RaycastDirectionMode CurrentRaycastDirectionMode { get => currentRaycastDirectionMode; set => currentRaycastDirectionMode = value; } [SerializeField] [Tooltip("How solver will orient model. None = no orienting, TrackedTarget = Face tracked target transform, SurfaceNormal = Aligned to surface normal completely, Blended = blend between tracked transform and surface orientation")] private OrientationMode orientationMode = OrientationMode.TrackedTarget; /// /// How solver will orient model. See OrientationMode enum for possible modes. When mode=Blended, use OrientationBlend property to define ratio for blending /// public OrientationMode CurrentOrientationMode { get => orientationMode; set => orientationMode = value; } [SerializeField] [Tooltip("Value used for when OrientationMode=Blended. If 0.0 orientation is driven as if in TrackedTarget mode, and if 1.0 orientation is driven as if in SurfaceNormal mode")] private float orientationBlend = 0.65f; /// /// Value used for when Orientation Mode=Blended. If 0.0 orientation is driven all by TrackedTarget mode and if 1.0 orientation is driven all by SurfaceNormal mode /// public float OrientationBlend { get => orientationBlend; set => orientationBlend = value; } [SerializeField] [Tooltip("If true, ensures object is kept vertical for TrackedTarget, SurfaceNormal, and Blended Orientation Modes")] private bool keepOrientationVertical = true; /// /// If true, ensures object is kept vertical for TrackedTarget, SurfaceNormal, and Blended Orientation Modes /// public bool KeepOrientationVertical { get => keepOrientationVertical; set => keepOrientationVertical = value; } [SerializeField] [Tooltip("If enabled, the debug lines will be drawn in the editor")] private bool debugEnabled = false; /// /// If enabled, the debug lines will be drawn in the editor /// public bool DebugEnabled { get => debugEnabled; set => debugEnabled = value; } #endregion /// /// Whether or not the object is currently magnetized to a surface. /// public bool OnSurface { get; private set; } private const float MaxDot = 0.97f; private RayStep currentRayStep = new RayStep(); private BoxCollider boxCollider; private Vector3 RaycastOrigin => SolverHandler.TransformTarget == null ? Vector3.zero : SolverHandler.TransformTarget.position; /// /// Which point should the ray cast toward? Not really the 'end' of the ray. The ray may be cast along /// the head facing direction, from the eye to the object, or to the solver's linked position (working from /// the previous solvers) /// private Vector3 RaycastEndPoint { get { Vector3 origin = RaycastOrigin; Vector3 endPoint = Vector3.forward; switch (CurrentRaycastDirectionMode) { case RaycastDirectionMode.TrackedTargetForward: if (SolverHandler != null && SolverHandler.TransformTarget != null) { endPoint = SolverHandler.TransformTarget.position + SolverHandler.TransformTarget.forward; } break; case RaycastDirectionMode.ToObject: endPoint = transform.position; break; case RaycastDirectionMode.ToLinkedPosition: endPoint = SolverHandler.GoalPosition; break; } return endPoint; } } /// /// Calculate the raycast direction based on the two ray points /// private Vector3 RaycastDirection { get { Vector3 direction = Vector3.forward; if (CurrentRaycastDirectionMode == RaycastDirectionMode.TrackedTargetForward) { if (SolverHandler.TransformTarget != null) { direction = SolverHandler.TransformTarget.forward; } } else { direction = (RaycastEndPoint - RaycastOrigin).normalized; } return direction; } } /// /// A constant scale override may be specified for volumetric raycasts, otherwise uses the current value of the solver link's alt scale /// private float ScaleOverride => useLinkedAltScaleOverride ? SolverHandler.AltScale.Current.magnitude : volumeCastSizeOverride; /// /// Calculates how the object should orient to the surface. /// /// direction of tracked target /// normal of surface at hit point /// Quaternion, the orientation to use for the object private Quaternion CalculateMagnetismOrientation(Vector3 direction, Vector3 surfaceNormal) { // Compute the up vector of our current working rotation, // to avoid gimbal lock instability when normal is also pointing upwards. // This "current" up vector is used in the LookRotation, which causes // the derived rotation to fit "closest" to the current up vector. Vector3 currentUpVector = WorkingRotation * Vector3.up; Quaternion trackedReferenceRotation = Quaternion.LookRotation(-direction, currentUpVector); Quaternion surfaceReferenceRotation = Quaternion.LookRotation(-surfaceNormal, currentUpVector); // If requested, compute FromTo from the current computed Up to global Up, // and apply to the computed quat; this will ensure object stays globally vertical. if (KeepOrientationVertical) { Vector3 trackedReferenceUp = trackedReferenceRotation * Vector3.up; trackedReferenceRotation = Quaternion.FromToRotation(trackedReferenceUp, Vector3.up) * trackedReferenceRotation; Vector3 surfaceReferenceUp = surfaceReferenceRotation * Vector3.up; surfaceReferenceRotation = Quaternion.FromToRotation(surfaceReferenceUp, Vector3.up) * surfaceReferenceRotation; } switch (CurrentOrientationMode) { case OrientationMode.None: return SolverHandler.GoalRotation; case OrientationMode.TrackedTarget: return trackedReferenceRotation; case OrientationMode.SurfaceNormal: return surfaceReferenceRotation; case OrientationMode.Blended: return Quaternion.Slerp(trackedReferenceRotation, surfaceReferenceRotation, orientationBlend); case OrientationMode.TrackedOrigin: return Quaternion.LookRotation(direction, Vector3.up); default: return Quaternion.identity; } } /// public override void SolverUpdate() { // Pass-through by default GoalPosition = WorkingPosition; GoalRotation = WorkingRotation; // Determine raycast params. Update struct to skip instantiation Vector3 origin = RaycastOrigin; Vector3 endpoint = RaycastEndPoint; currentRayStep.UpdateRayStep(ref origin, ref endpoint); // Skip if there isn't a valid direction if (currentRayStep.Direction == Vector3.zero) { return; } if (DebugEnabled) { Debug.DrawLine(currentRayStep.Origin, currentRayStep.Terminus, Color.magenta); } switch (RaycastMode) { case SceneQueryType.SimpleRaycast: SimpleRaycastStepUpdate(ref this.currentRayStep); break; case SceneQueryType.BoxRaycast: BoxRaycastStepUpdate(ref this.currentRayStep); break; case SceneQueryType.SphereCast: SphereRaycastStepUpdate(ref this.currentRayStep); break; case SceneQueryType.SphereOverlap: Debug.LogError("Raycast mode set to SphereOverlap which is not valid for SurfaceMagnetism component. Disabling update solvers..."); SolverHandler.UpdateSolvers = false; break; } } /// /// Calculate solver for simple raycast with provided ray /// /// start/end ray passed by read-only reference to avoid struct-copy performance private void SimpleRaycastStepUpdate(ref RayStep rayStep) { bool isHit; RaycastHit result; // Do the cast! isHit = MixedRealityRaycaster.RaycastSimplePhysicsStep(rayStep, maxRaycastDistance, magneticSurfaces, false, out result); OnSurface = isHit; // Enforce CloseDistance Vector3 hitDelta = result.point - rayStep.Origin; float length = hitDelta.magnitude; if (length < closestDistance) { result.point = rayStep.Origin + rayStep.Direction * closestDistance; } // Apply results if (isHit) { GoalPosition = result.point + surfaceNormalOffset * result.normal + surfaceRayOffset * rayStep.Direction; GoalRotation = CalculateMagnetismOrientation(rayStep.Direction, result.normal); } } /// /// Calculate solver for sphere raycast with provided ray /// /// start/end ray passed by read-only reference to avoid struct-copy performance private void SphereRaycastStepUpdate(ref RayStep rayStep) { bool isHit; RaycastHit result; // Do the cast! float size = ScaleOverride > 0 ? ScaleOverride : transform.lossyScale.x * sphereSize; isHit = MixedRealityRaycaster.RaycastSpherePhysicsStep(rayStep, size, maxRaycastDistance, magneticSurfaces, false, out result); OnSurface = isHit; // Enforce CloseDistance Vector3 hitDelta = result.point - rayStep.Origin; float length = hitDelta.magnitude; if (length < closestDistance) { result.point = rayStep.Origin + rayStep.Direction * closestDistance; } // Apply results if (isHit) { GoalPosition = result.point + surfaceNormalOffset * result.normal + surfaceRayOffset * rayStep.Direction; GoalRotation = CalculateMagnetismOrientation(rayStep.Direction, result.normal); } } /// /// Calculate solver for box raycast with provided ray /// /// start/end ray passed by read-only reference to avoid struct-copy performance private void BoxRaycastStepUpdate(ref RayStep rayStep) { Vector3 scale = ScaleOverride > 0 ? transform.lossyScale.normalized * ScaleOverride : transform.lossyScale; Quaternion orientation = orientationMode == OrientationMode.None ? Quaternion.LookRotation(rayStep.Direction, Vector3.up) : CalculateMagnetismOrientation(rayStep.Direction, Vector3.up); Matrix4x4 targetMatrix = Matrix4x4.TRS(Vector3.zero, orientation, scale); if (this.boxCollider == null) { this.boxCollider = GetComponent(); } Debug.Assert(boxCollider != null, $"Missing a box collider for Surface Magnetism on {gameObject}"); Vector3 extents = boxCollider.size; Vector3[] positions; Vector3[] normals; bool[] hits; if (MixedRealityRaycaster.RaycastBoxPhysicsStep(rayStep, extents, transform.position, targetMatrix, maxRaycastDistance, magneticSurfaces, boxRaysPerEdge, orthographicBoxCast, false, out positions, out normals, out hits)) { Plane plane; float distance; // Place an unconstrained plane down the ray. Don't use vertical constrain. FindPlacementPlane(rayStep.Origin, rayStep.Direction, positions, normals, hits, boxCollider.size.x, maximumNormalVariance, false, orientationMode == OrientationMode.None, out plane, out distance); // If placing on a horizontal surface, need to adjust the calculated distance by half the app height float verticalCorrectionOffset = 0; if (IsNormalVertical(plane.normal) && !Mathf.Approximately(rayStep.Direction.y, 0)) { float boxSurfaceVerticalOffset = targetMatrix.MultiplyVector(new Vector3(0, extents.y * 0.5f, 0)).magnitude; Vector3 correctionVector = boxSurfaceVerticalOffset * (rayStep.Direction / rayStep.Direction.y); verticalCorrectionOffset = -correctionVector.magnitude; } float boxSurfaceOffset = targetMatrix.MultiplyVector(new Vector3(0, 0, extents.z * 0.5f)).magnitude; // Apply boxSurfaceOffset to ray direction and not surface normal direction to reduce sliding GoalPosition = rayStep.Origin + rayStep.Direction * Mathf.Max(closestDistance, distance + surfaceRayOffset + boxSurfaceOffset + verticalCorrectionOffset) + plane.normal * (0 * boxSurfaceOffset + surfaceNormalOffset); GoalRotation = CalculateMagnetismOrientation(rayStep.Direction, plane.normal); OnSurface = true; } else { OnSurface = false; } } /// /// Calculates a plane from all raycast hit locations upon which the object may align. Used in Box Raycast Mode. /// private void FindPlacementPlane(Vector3 origin, Vector3 direction, Vector3[] positions, Vector3[] normals, bool[] hits, float assetWidth, float maxNormalVariance, bool constrainVertical, bool useClosestDistance, out Plane plane, out float closestDistance) { int rayCount = positions.Length; Vector3 originalDirection = direction; if (constrainVertical) { direction.y = 0.0f; direction = direction.normalized; } // Go through all the points and find the closest distance closestDistance = float.PositiveInfinity; int numHits = 0; int closestPointIdx = -1; float farthestDistance = 0f; var averageNormal = Vector3.zero; for (int hitIndex = 0; hitIndex < rayCount; hitIndex++) { if (hits[hitIndex]) { float distance = Vector3.Dot(direction, positions[hitIndex] - origin); if (distance < closestDistance) { closestPointIdx = hitIndex; closestDistance = distance; } if (distance > farthestDistance) { farthestDistance = distance; } averageNormal += normals[hitIndex]; ++numHits; } } Vector3 closestPoint = positions[closestPointIdx]; averageNormal /= numHits; // Calculate variance of all normals float variance = 0; for (int hitIndex = 0; hitIndex < rayCount; ++hitIndex) { if (hits[hitIndex]) { variance += (normals[hitIndex] - averageNormal).magnitude; } } variance /= numHits; // If variance is too high, I really don't want to deal with this surface // And if we don't even have enough rays, I'm not confident about this at all if (variance > maxNormalVariance || numHits < rayCount * 0.25f) { plane = new Plane(-direction, closestPoint); return; } // go through all the points and find the most orthogonal plane var lowAngle = float.PositiveInfinity; var highAngle = float.NegativeInfinity; int lowIndex = -1; int highIndex = -1; for (int hitIndex = 0; hitIndex < rayCount; hitIndex++) { if (hits[hitIndex] == false || hitIndex == closestPointIdx) { continue; } Vector3 difference = positions[hitIndex] - closestPoint; if (constrainVertical) { difference.y = 0.0f; difference.Normalize(); if (difference == Vector3.zero) { continue; } } difference.Normalize(); float angle = Vector3.Dot(direction, difference); if (angle < lowAngle) { lowAngle = angle; lowIndex = hitIndex; } } if (!constrainVertical && lowIndex != -1) { for (int hitIndex = 0; hitIndex < rayCount; hitIndex++) { if (hits[hitIndex] == false || hitIndex == closestPointIdx || hitIndex == lowIndex) { continue; } float dot = Mathf.Abs(Vector3.Dot((positions[hitIndex] - closestPoint).normalized, (positions[lowIndex] - closestPoint).normalized)); if (dot > MaxDot) { continue; } float nextAngle = Mathf.Abs(Vector3.Dot(direction, Vector3.Cross(positions[lowIndex] - closestPoint, positions[hitIndex] - closestPoint).normalized)); if (nextAngle > highAngle) { highAngle = nextAngle; highIndex = hitIndex; } } } Vector3 placementNormal; if (lowIndex != -1) { if (debugEnabled) { Debug.DrawLine(closestPoint, positions[lowIndex], Color.red); } if (highIndex != -1) { if (debugEnabled) { Debug.DrawLine(closestPoint, positions[highIndex], Color.green); } placementNormal = Vector3.Cross(positions[lowIndex] - closestPoint, positions[highIndex] - closestPoint).normalized; } else { Vector3 planeUp = Vector3.Cross(positions[lowIndex] - closestPoint, direction); placementNormal = Vector3.Cross(positions[lowIndex] - closestPoint, constrainVertical ? Vector3.up : planeUp).normalized; } if (debugEnabled) { Debug.DrawLine(closestPoint, closestPoint + placementNormal, Color.blue); } } else { placementNormal = direction * -1.0f; } if (Vector3.Dot(placementNormal, direction) > 0.0f) { placementNormal *= -1.0f; } plane = new Plane(placementNormal, closestPoint); if (debugEnabled) { Debug.DrawRay(closestPoint, placementNormal, Color.cyan); } // Figure out how far the plane should be. if (!useClosestDistance && closestPointIdx >= 0) { float centerPlaneDistance; if (plane.Raycast(new Ray(origin, originalDirection), out centerPlaneDistance) || !centerPlaneDistance.Equals(0.0f)) { // When the plane is nearly parallel to the user, we need to clamp the distance to where the raycasts hit. closestDistance = Mathf.Clamp(centerPlaneDistance, closestDistance, farthestDistance + assetWidth * 0.5f); } else { Debug.LogError("FindPlacementPlane: Not expected to have the center point not intersect the plane."); } } } /// /// Checks if a normal is nearly vertical /// /// Returns true, if normal is vertical. private static bool IsNormalVertical(Vector3 normal) => 1f - Mathf.Abs(normal.y) < 0.01f; #region Obsolete /// /// Max distance for raycast to check for surfaces /// [Obsolete("Use MaxRaycastDistance instead")] public float MaxDistance { get => maxRaycastDistance; set => maxRaycastDistance = value; } /// /// Closest distance to bring object /// [Obsolete("Use ClosestDistance instead")] public float CloseDistance { get { return closestDistance; } set { closestDistance = value; } } #endregion } }