ApexSurgery/Assets/Imstk/Scripts/Interactions/Grasping.cs

/*=========================================================================

   Library: iMSTK-Unity

   Copyright (c) Kitware, Inc. 

   Licensed under the Apache License, Version 2.0 (the "License");
   you may not use this file except in compliance with the License.
   You may obtain a copy of the License at

      http://www.apache.org/licenses/LICENSE-2.0.txt

   Unless required by applicable law or agreed to in writing, software
   distributed under the License is distributed on an "AS IS" BASIS,
   WITHOUT WARRANTIES OR CONDITIONS OF ANY KIND, either express or implied.
   See the License for the specific language governing permissions and
   limitations under the License.

=========================================================================*/

using UnityEngine;

namespace ImstkUnity
{
    /// <summary>
    /// This class is used to enable grasping between a rigid object and a deformable.
    /// Assigning a rigid and a deformable, then use <c>StartGrasp()</c> and <c>EndGrasp()</c>
    /// to drive the grasping. <c>useVertexGrasping</c> indicates that the grasped item on
    /// the deformable will be a vertex, otherwise it will be a whole cell (e.g. triangle)
    /// When grasped iMSTK will attempt to maintain the relation between the grasped and the
    /// grasping geometry. When the rigid is moved the grasped side will attempt to move with
    /// it.
    /// </summary>
    public class Grasping : ImstkInteractionBehaviour
    {
        public enum GraspType
        {
            Vertex,
            Cell,
        }
        public Rigid rigidModel;
        public GeometryFilter graspingGeometry;
        public DynamicalModel graspedObject;
        [Tooltip ("If not null the collision will be disabled when a grasp happened")]
        public CollisionInteraction collisionToDisable;

        public GraspType graspType = GraspType.Cell;

        Imstk.PbdObjectGrasping interaction;
        private string _collisionDetectionType;

        public double deformableGraspingStiffness = 0.4;
        public double rigidGraspingStiffness = 1 / 0.0001;

        private void OnEnable()
        {
            
        }

        bool OneIsA<T>(DynamicalModel a, DynamicalModel b) where T : DynamicalModel
        {
            if (b as T != null) return true;
            if (a as T != null) return true;
            return false;
        }

        public override Imstk.SceneObject GetImstkInteraction()
        {
            if (rigidModel == null || graspedObject == null)
            {
                Debug.LogWarning("Both models need to be assigned for the Grasping Interaction to work");
                enabled = false;
                return null;
            }

            _collisionDetectionType = CollisionInteraction.GetCDType(rigidModel, graspedObject);

            if (_collisionDetectionType == "")
            {
                Debug.LogError("Could not determine collision type for grasping in " + gameObject.name);
            }

            if (! rigidModel.isActiveAndEnabled || !graspedObject.isActiveAndEnabled)
            {
                return null;
            }

            interaction = new Imstk.PbdObjectGrasping(graspedObject.GetDynamicObject() as Imstk.PbdObject,
                rigidModel.GetDynamicObject() as Imstk.PbdObject);

            interaction.setStiffness(deformableGraspingStiffness);
            interaction.setCompliance(1 / rigidGraspingStiffness);

            Imstk.Geometry geom = rigidModel.GetDynamicObject().getCollidingGeometry();
 
            Imstk.AnalyticalGeometry analytical = Imstk.Utils.CastTo<Imstk.AnalyticalGeometry>(geom);

            if (analytical == null)
            {
                Debug.LogError("Can't convert to analytical geometry" + geom.getTypeName());
            }

            return interaction;
        }

        public void StartGrasp()
        {
            if (interaction == null) return;

            Imstk.Geometry rigidGeometry = rigidModel.GetDynamicObject().getCollidingGeometry();
            Imstk.AnalyticalGeometry analyticalGraspingGeom;
            if (graspingGeometry == null)
            {
                analyticalGraspingGeom = Imstk.Utils.CastTo<Imstk.AnalyticalGeometry>(rigidGeometry);
            } 
            else
            {
                analyticalGraspingGeom = Imstk.Utils.CastTo<Imstk.AnalyticalGeometry>(graspingGeometry.GetOutputGeometry());
                analyticalGraspingGeom.setTransform(rigidGeometry.getTransform());
                analyticalGraspingGeom.updatePostTransformData();
            }

            if (analyticalGraspingGeom == null)
            {
                Debug.LogError("Grasping Geometry can't be null in " + gameObject.name);
                return;
            }

            switch (graspType)
            {
                case (GraspType.Cell):
                    interaction.beginCellGrasp(analyticalGraspingGeom);
                    break;
                case (GraspType.Vertex):
                    interaction.beginVertexGrasp(analyticalGraspingGeom);
                    break;
            }
        }

        private void Update()
        {
            if (interaction != null && collisionToDisable != null)
            {
                var imstkCollision = Imstk.Utils.CastTo<Imstk.CollisionInteraction>(collisionToDisable.GetImstkInteraction());
                if (interaction.hasConstraints())
                {
                    imstkCollision.setEnabled(false);
                }
                else
                {
                    imstkCollision.setEnabled(true);
                }
            }
        }

        public void EndGrasp()
        {
            if (interaction != null)
            {
                interaction.endGrasp();
            }
        }

        /// <summary>
        /// Will return true whenever constraints where generated, this means
        /// that something has _actually_ been grasped.
        /// NOTE it takes at least one simulation manager FixedUpdate() for this
        /// to return a correct value, check in the next frame after calling 
        /// StartGrasp()
        /// </summary>
        public bool HasConstraints()
        {
            return interaction != null && interaction.hasConstraints();
        }
    }

}