Initial Commit

Assets/Imstk/Scripts/Interactions/Grasping.cs

@@ -0,0 +1,177 @@
+/*=========================================================================
+
+   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();
+        }
+    }
+
+}