SolverSurfaceMagnetism.cs

Go to the documentation of this file.

// Copyright (c) Microsoft Corporation. All rights reserved.
// Licensed under the MIT License. See LICENSE in the project root for license information.

using UnityEngine;

namespace HoloToolkit.Unity
{
    public class SolverSurfaceMagnetism : Solver
    {
        #region public enums
        public enum RaycastDirectionEnum
        {
            CameraFacing,
            ToObject,
            ToLinkedPosition
        }
        public enum RaycastModeEnum
        {
            Simple,
            Box,
            Sphere
        }

        public enum OrientModeEnum
        {
            None,
            Vertical,
            Full,
            Blended
        }
        #endregion

        #region public members
        [Tooltip("LayerMask to apply Surface Magnetism to")]
        public LayerMask MagneticSurface = 0;

        [Tooltip("Max distance to check for surfaces")]
        public float MaxDistance = 3.0f;
        [Tooltip("Closest distance to bring object")]
        public float CloseDistance = 0.5f;

        [Tooltip("Offset from surface along surface normal")]
        public float SurfaceNormalOffset = 0.5f;
        [Tooltip("Offset from surface along ray cast direction")]
        public float SurfaceRayOffset = 0;

        [Tooltip("Surface raycast mode.  Simple = single raycast, Complex = bbox corners")]
        public RaycastModeEnum raycastMode = RaycastModeEnum.Simple;

        [Tooltip("Number of rays per edge, should be odd. Total casts is n^2")]
        public int BoxRaysPerEdge = 3;

        [Tooltip("If true, use orthographic casting for box lines instead of perspective")]
        public bool OrthoBoxCast = false;

        [Tooltip("Align to ray cast direction if box cast hits many normals facing in varying directions")]
        public float MaximumNormalVariance = 0.5f;

        [Tooltip("Radius to use for sphere cast")]
        public float SphereSize = 1.0f;

        [Tooltip("When doing volume casts, use size override if non-zero instead of object's current scale")]
        public float VolumeCastSizeOverride = 0;

        [Tooltip("When doing volume casts, use linked AltScale instead of object's current scale")]
        public bool UseLinkedAltScaleOverride = false;

        // This is broken
        [Tooltip("Instead of using mesh normal, extract normal from tex coord (SR is reported to put smoothed normals in there)")]
        bool UseTexCoordNormals = false;

        [Tooltip("Raycast direction.  Can cast from head in facing dir, or cast from head to object position")]
        public RaycastDirectionEnum raycastDirection = RaycastDirectionEnum.ToLinkedPosition;

        [Tooltip("Orientation mode.  None = no orienting, Vertical = Face head, but always oriented up/down, Full = Aligned to surface normal completely")]
        public OrientModeEnum orientationMode = OrientModeEnum.Vertical;

        [Tooltip("Orientation Blend Value 0.0 = All head 1.0 = All surface")]
        public float OrientBlend = 0.65f;

        [HideInInspector]
        public bool OnSurface;
        #endregion

        #region private members
        private BoxCollider m_BoxCollider;
        private const float maxDot = 0.97f;
        #endregion

        protected void Start()
        {
            if (raycastMode == RaycastModeEnum.Box)
            {
                m_BoxCollider = GetComponent<BoxCollider>();
                if (m_BoxCollider == null)
                {
                    Debug.LogError("Box raycast mode requires a BoxCollider, but none was found!  Defaulting to Simple raycast mode");
                    raycastMode = RaycastModeEnum.Simple;
                }

                if (Application.isEditor)
                {
                    RaycastHelper.DebugEnabled = true;
                }
            }

            if (Application.isEditor && UseTexCoordNormals)
            {
                Debug.LogWarning("Disabling tex coord normals while in editor mode");
                UseTexCoordNormals = false;
            }
        }

        private static bool DefaultRaycast(Vector3 origin, Vector3 direction, float distance, LayerMask surface, out RaycastResultHelper result)
        {
            return RaycastHelper.First(origin, direction, distance, surface, out result);
        }

        private static bool DefaultSpherecast(Vector3 origin, Vector3 direction, float radius, float distance, LayerMask surface, out RaycastResultHelper result)
        {
            return RaycastHelper.SphereFirst(origin, direction, radius, distance, surface, out result);
        }

        Vector3 GetRaycastOrigin()
        {
            if (solverHandler.TransformTarget == null)
            {
                return Vector3.zero;
            }
            return solverHandler.TransformTarget.position;
        }

        Vector3 GetRaycastEndPoint()
        {
            Vector3 ret = Vector3.forward;
            switch (raycastDirection)
            {
                case RaycastDirectionEnum.CameraFacing:
                    ret = solverHandler.TransformTarget.position + solverHandler.TransformTarget.forward;
                    break;

                case RaycastDirectionEnum.ToObject:
                    ret = transform.position;
                    break;

                case RaycastDirectionEnum.ToLinkedPosition:
                    ret = solverHandler.GoalPosition;
                    break;
            }
            return ret;
        }

        Vector3 GetRaycastDirection()
        {
            Vector3 ret = Vector3.forward;
            if (raycastDirection == RaycastDirectionEnum.CameraFacing)
            {

                if (solverHandler.TransformTarget)
                {
                    ret = solverHandler.TransformTarget.forward;
                }
            }
            else
            {
                ret = (GetRaycastEndPoint() - GetRaycastOrigin()).normalized;
            }
            return ret;
        }

        Quaternion CalculateMagnetismOrientation(Vector3 rayDir, Vector3 surfaceNormal)
        {
            // Calculate the surface rotation
            Vector3 newDir = -surfaceNormal;
            if (IsNormalVertical(newDir))
            {
                newDir = rayDir;
            }

            newDir.y = 0;

            Quaternion surfaceRot = Quaternion.LookRotation(newDir, Vector3.up);

            switch (orientationMode)
            {
                case OrientModeEnum.None:
                    return solverHandler.GoalRotation;

                case OrientModeEnum.Vertical:
                    return surfaceRot;

                case OrientModeEnum.Full:
                    return Quaternion.LookRotation(-surfaceNormal, Vector3.up);

                case OrientModeEnum.Blended:
                    return Quaternion.Slerp(solverHandler.GoalRotation, surfaceRot, OrientBlend);
                default:
                    return Quaternion.identity;
            }
        }

        bool IsNormalVertical(Vector3 normal)
        {
            return 1f - Mathf.Abs(normal.y) < 0.01f;
        }

        float GetScaleOverride()
        {
            if (UseLinkedAltScaleOverride)
            {
                return solverHandler.AltScale.Current.magnitude;
            }
            return VolumeCastSizeOverride;
        }

        public override void SolverUpdate()
        {
            // Pass-through by default
            this.GoalPosition = WorkingPos;
            this.GoalRotation = WorkingRot;

            // Determine raycast params
            Ray ray = new Ray(GetRaycastOrigin(), GetRaycastDirection());

            // Skip if there's no valid direction
            if (ray.direction == Vector3.zero)
            {
                return;
            }

            float ScaleOverride = GetScaleOverride();
            float len;
            bool bHit;
            RaycastResultHelper result;
            Vector3 hitDelta;

            switch (raycastMode)
            {
                case RaycastModeEnum.Simple:
                default:

                    // Do the cast!
                    bHit = DefaultRaycast(ray.origin, ray.direction, MaxDistance, MagneticSurface, out result);

                    OnSurface = bHit;

                    if (UseTexCoordNormals)
                    {
                        result.OverrideNormalFromTextureCoord();
                    }

                    // Enforce CloseDistance
                    hitDelta = result.Point - ray.origin;
                    len = hitDelta.magnitude;
                    if (len < CloseDistance)
                    {
                        result.OverridePoint(ray.origin + ray.direction * CloseDistance);
                    }

                    // Apply results
                    if (bHit)
                    {
                        GoalPosition = result.Point + SurfaceNormalOffset * result.Normal + SurfaceRayOffset * ray.direction;
                        GoalRotation = CalculateMagnetismOrientation(ray.direction, result.Normal);
                    }
                    break;

                case RaycastModeEnum.Box:

                    Vector3 scale = transform.lossyScale;
                    if (ScaleOverride > 0)
                    {
                        scale = scale.normalized * ScaleOverride;
                    }

                    Quaternion orientation = orientationMode == OrientModeEnum.None ? Quaternion.LookRotation(ray.direction, Vector3.up) : CalculateMagnetismOrientation(ray.direction, Vector3.up);
                    Matrix4x4 targetMatrix = Matrix4x4.TRS(Vector3.zero, orientation, scale);

                    if (m_BoxCollider == null)
                    {
                        m_BoxCollider = this.GetComponent<BoxCollider>();
                    }

                    Vector3 extents = m_BoxCollider.size;

                    Vector3[] positions;
                    Vector3[] normals;
                    bool[] hits;

                    if (RaycastHelper.CastBoxExtents(extents, transform.position, targetMatrix, ray, MaxDistance, MagneticSurface, DefaultRaycast, BoxRaysPerEdge, OrthoBoxCast, out positions, out normals, out hits))
                    {
                        Plane plane;
                        float distance;

                        // place an unconstrained plane down the ray.  Never use vertical constrain.
                        FindPlacementPlane(ray.origin, ray.direction, positions, normals, hits, m_BoxCollider.size.x, MaximumNormalVariance, false, orientationMode == OrientModeEnum.None, out plane, out distance);

                        // If placing on a horzizontal surface, need to adjust the calculated distance by half the app height
                        float verticalCorrectionOffset = 0;
                        if (IsNormalVertical(plane.normal) && !Mathf.Approximately(ray.direction.y, 0))
                        {
                            float boxSurfaceOffsetVert = targetMatrix.MultiplyVector(new Vector3(0, extents.y / 2f, 0)).magnitude;
                            Vector3 correctionVec = boxSurfaceOffsetVert * (ray.direction / ray.direction.y);
                            verticalCorrectionOffset = -correctionVec.magnitude;
                        }

                        float boxSurfaceOffset = targetMatrix.MultiplyVector(new Vector3(0, 0, extents.z / 2f)).magnitude;

                        // Apply boxSurfaceOffset to rayDir and not surfaceNormalDir to reduce sliding
                        GoalPosition = ray.origin + ray.direction * Mathf.Max(CloseDistance, distance + SurfaceRayOffset + boxSurfaceOffset + verticalCorrectionOffset) + plane.normal * (0 * boxSurfaceOffset + SurfaceNormalOffset);
                        GoalRotation = CalculateMagnetismOrientation(ray.direction, plane.normal);
                        OnSurface = true;
                    }
                    else
                    {
                        OnSurface = false;
                    }
                    break;

                case RaycastModeEnum.Sphere:

                    // Do the cast!
                    float size = ScaleOverride > 0 ? ScaleOverride : transform.lossyScale.x * SphereSize;
                    bHit = DefaultSpherecast(ray.origin, ray.direction, size, MaxDistance, MagneticSurface, out result);
                    OnSurface = bHit;

                    // Enforce CloseDistance
                    hitDelta = result.Point - ray.origin;
                    len = hitDelta.magnitude;
                    if (len < CloseDistance)
                    {
                        result.OverridePoint(ray.origin + ray.direction * CloseDistance);
                    }

                    // Apply results
                    if (bHit)
                    {
                        GoalPosition = result.Point + SurfaceNormalOffset * result.Normal + SurfaceRayOffset * ray.direction;
                        GoalRotation = CalculateMagnetismOrientation(ray.direction, result.Normal);
                    }
                    break;
            }

            // Do frame to frame updates of transform, smoothly toward the goal, if desired
            UpdateWorkingPosToGoal();
            UpdateWorkingRotToGoal();
        }

        private static void FindPlacementPlane(Vector3 origin, Vector3 direction, Vector3[] positions, Vector3[] normals, bool[] hits, float assetWidth, float maxNormalVariance, bool constrainVertical, bool bUseClosestDistance, out Plane plane, out float closestDistance)
        {
            bool debugEnabled = RaycastHelper.DebugEnabled;

            int numRays = positions.Length;

            Vector3 originalDirection = direction;
            if (constrainVertical)
            {
                direction.y = 0.0f;
                direction = direction.normalized;
            }

            // go through all the points and find the closest distance
            int closestPoint = -1;
            closestDistance = float.PositiveInfinity;
            float farthestDistance = 0f;
            int numHits = 0;
            Vector3 averageNormal = Vector3.zero;

            for (int i = 0; i < numRays; i++)
            {
                if (hits[i] != false)
                {
                    float dist = Vector3.Dot(direction, positions[i] - origin);

                    if (dist < closestDistance)
                    {
                        closestPoint = i;
                        closestDistance = dist;
                    }
                    if (dist > farthestDistance)
                    {
                        farthestDistance = dist;
                    }

                    averageNormal += normals[i];
                    ++numHits;
                }
            }
            averageNormal /= numHits;

            // Calculate variance of all normals
            float variance = 0;
            for (int i = 0; i < numRays; ++i)
            {
                if (hits[i] != false)
                {
                    variance += (normals[i] - 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 < numRays / 4)
            {
                plane = new Plane(-direction, positions[closestPoint]);
                return;
            }

            // go through all the points and find the most orthagonal plane
            float lowAngle = float.PositiveInfinity;
            int lowIndex = -1;
            float highAngle = float.NegativeInfinity;
            int highIndex = -1;

            for (int i = 0; i < numRays; i++)
            {
                if (hits[i] == false || i == closestPoint)
                {
                    continue;
                }

                Vector3 diff = (positions[i] - positions[closestPoint]);
                if (constrainVertical)
                {
                    diff.y = 0.0f;
                    diff.Normalize();

                    if (diff == Vector3.zero)
                    {
                        continue;
                    }
                }
                else
                {
                    diff.Normalize();
                }

                float angle = Vector3.Dot(direction, diff);

                if (angle < lowAngle)
                {
                    lowAngle = angle;
                    lowIndex = i;
                }
            }

            if (!constrainVertical && lowIndex != -1)
            {
                for (int i = 0; i < numRays; i++)
                {
                    if (hits[i] == false || i == closestPoint || i == lowIndex)
                    {
                        continue;
                    }

                    float dot = Mathf.Abs(Vector3.Dot((positions[i] - positions[closestPoint]).normalized, (positions[lowIndex] - positions[closestPoint]).normalized));
                    if (dot > maxDot)
                    {
                        continue;
                    }

                    Vector3 normal = Vector3.Cross(positions[lowIndex] - positions[closestPoint], positions[i] - positions[closestPoint]).normalized;

                    float nextAngle = Mathf.Abs(Vector3.Dot(direction, normal));

                    if (nextAngle > highAngle)
                    {
                        highAngle = nextAngle;
                        highIndex = i;
                    }
                }
            }

            Vector3 placementNormal;
            if (lowIndex != -1)
            {
                if (debugEnabled)
                {
                    Debug.DrawLine(positions[closestPoint], positions[lowIndex], Color.red);
                }

                if (highIndex != -1)
                {
                    if (debugEnabled)
                    {
                        Debug.DrawLine(positions[closestPoint], positions[highIndex], Color.green);
                    }
                    placementNormal = Vector3.Cross(positions[lowIndex] - positions[closestPoint], positions[highIndex] - positions[closestPoint]).normalized;
                }
                else
                {
                    Vector3 planeUp = Vector3.Cross(positions[lowIndex] - positions[closestPoint], direction);
                    placementNormal = Vector3.Cross(positions[lowIndex] - positions[closestPoint], constrainVertical ? Vector3.up : planeUp).normalized;
                }

                if (debugEnabled)
                {
                    Debug.DrawLine(positions[closestPoint], positions[closestPoint] + placementNormal, Color.blue);
                }
            }
            else
            {
                placementNormal = direction * -1.0f;
            }

            if (Vector3.Dot(placementNormal, direction) > 0.0f)
            {
                placementNormal *= -1.0f;
            }

            plane = new Plane(placementNormal, positions[closestPoint]);

            if (debugEnabled)
            {
                Debug.DrawRay(positions[closestPoint], placementNormal, Color.cyan);
            }

            // Figure out how far the plane should be.
            if (!bUseClosestDistance && closestPoint >= 0)
            {
                float centerPlaneDistance;
                Ray centerPlaneRay = new Ray(origin, originalDirection);
                if (plane.Raycast(centerPlaneRay, out centerPlaneDistance) || centerPlaneDistance != 0)
                {
                    // 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.");
                }
            }
        }
    }
}