Initial Commit

Assets/Imstk/Scripts/Interactions/CollisionInteraction.cs

@@ -0,0 +1,189 @@
+/*=========================================================================
+
+   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 System;
+using UnityEngine;
+
+namespace ImstkUnity
+{
+    // Still need to manually maintain this list if users want to 
+    // Select an explicit 
+    public enum StandardCollisionTypes
+    {
+        Auto,
+        BidirectionalPlaneToSphereCD,
+        ImplicitGeometryToPointSetCCD,
+        ImplicitGeometryToPointSetCD,
+        MeshToMeshBruteForceCD,
+        PointSetToCapsuleCD,
+        PointSetToOrientedBoxCD,
+        PointSetToPlaneCD,
+        PointSetToSphereCD,
+        SphereToCylinderCD,
+        SphereToSphereCD,
+        SurfaceMeshToCapsuleCD,
+        SurfaceMeshToSphereCD,
+        SurfaceMeshToSurfaceMeshCD,
+        TetraToLineMeshCD,
+        TetraToPointSetCD,
+        UnidirectionalPlaneToSphereCD
+    };
+
+
+
+    /// <summary>
+    /// Convience collision interaction, uses a lot of defaults. If specifics needed
+    /// use the specific subclasses
+    /// </summary>
+    [AddComponentMenu("iMSTK/CollisionInteraction")]
+    public class CollisionInteraction : ImstkInteractionBehaviour
+    {
+        [Obsolete("The collisionType parameter should not be used anymore")]
+        public StandardCollisionTypes collisionType = StandardCollisionTypes.Auto;
+        
+        public string collisionTypeName = "Auto";
+        public DynamicalModel model1;
+        public DynamicalModel model2;
+
+        Imstk.CollisionInteraction interaction;
+
+        public double friction = 0.0;
+        public double restitution = 0.0;
+
+        [Tooltip("A good default value is 1/number of iterations from the simulation manager")]
+        public double deformableStiffness1 = 0.2;
+        [Tooltip("A good default value is 1/number of iterations from the simulation manager")]
+        public double deformableStiffness2 = 0.2;
+        public double rigidBodyCompliance = 0.0001;
+
+        /// <summary>
+        /// Start intentionally empty, including this enables the "enabled" check-box in the 
+        /// editor, allowing the activation/deactivation of this component in the GUI
+        /// </summary>
+        void Start()
+        {
+
+        }
+
+        // Create the Imstk interaction for this collision interaction
+        public override Imstk.SceneObject GetImstkInteraction()
+        {
+            if (interaction != null) return interaction;
+            if (model2 == null || model1 == null)
+            {
+                Debug.LogWarning("Interaction on object " + gameObject.name + " which does not have a DynamicalModel");
+                return null;
+            }
+
+            if (!model1.isActiveAndEnabled || !model2.isActiveAndEnabled || !isActiveAndEnabled)
+            {
+                return null;
+            }
+
+            if (collisionTypeName == "Auto" || collisionTypeName == "")
+            {
+                collisionTypeName = GetCDType(model1, model2);
+                if (collisionTypeName == "")
+                {
+                    return null;
+                }
+                Debug.Log("Using " + collisionTypeName + " for " + ImstkUnity.Utility.GetFullName(model1) + " and " +
+                    ImstkUnity.Utility.GetFullName(model2));
+            }
+
+            // Right now just check for Rigid and Deformable
+            if (model1 is Rigid || model1 is Deformable)
+            {
+                interaction = CreateInteraction(model1, model2);
+            }
+            else if (model2 is Rigid || model2 is Deformable)
+            {
+                interaction = CreateInteraction(model2, model1);
+            }
+
+            if (interaction == null)
+            {
+                Debug.LogWarning("Creating collision between two unsupported types, at least one needs to be a Rigd or Deformable");
+            }
+
+            return interaction;
+        }
+        /// <summary>
+        /// Look up the collision detection type in the iMSTK factory 
+        /// </summary>
+        /// <returns>A string with the deduced collision type, "" otherwise</returns>
+        public static string GetCDType(DynamicalModel model1, DynamicalModel model2)
+        {
+            string result = "";
+
+            if (model1 == null || model2 == null)
+            {
+                Debug.LogWarning("Can't determine collision one of the models is null");
+                return result;
+            }
+
+            Imstk.Geometry geom1 = model1.GetCollidingGeometry();
+            Imstk.Geometry geom2 = model2.GetCollidingGeometry();
+
+            if (geom1 == null)
+            {
+                Debug.LogError("Could not create collision interaction, " +
+                    model1.gameObject.name + "'s DynamicalModel does not contain a collision geometry");
+            }
+            if (geom2 == null)
+            {
+                Debug.LogError("Could not create collision interaction, " +
+                    model2.gameObject.name + "'s DynamicalModel does not contain a collision geometry");
+            }
+
+            result = Imstk.CDObjectFactory.getCDType(geom1, geom2);
+
+            return result;
+        }
+
+        private Imstk.CollisionInteraction CreateInteraction(DynamicalModel model1, DynamicalModel model2)
+        {
+            var pbd = Imstk.Utils.CastTo<Imstk.PbdObject>(model1.GetDynamicObject());
+            Debug.Assert(pbd != null);
+            // At the moment all colliding objects are PBD 
+            // PbdObjectCollision expects first parameter to be PBD, the second parameter
+            // may be any type of CollidingObj
+            Imstk.PbdObjectCollision collision =
+                new Imstk.PbdObjectCollision(
+                    pbd,
+                    model2.GetDynamicObject(),
+                    collisionTypeName);
+
+            // Just configure ALL the parameters
+            collision.setDeformableStiffnessA(deformableStiffness1);
+            collision.setDeformableStiffnessB(deformableStiffness2);
+            collision.setRigidBodyCompliance(rigidBodyCompliance);
+            collision.setFriction(friction);
+            collision.setRestitution(restitution);
+            collision.setUseCorrectVelocity(false);
+
+            Debug.Log("Created collision interaction of type: " + collision.GetType().Name + "for objects " +
+                model1.gameObject.name + " & " + model2.gameObject.name);
+
+            return collision;
+        }
+
+    }
+}

Assets/Imstk/Scripts/Interactions/CollisionInteraction.cs.meta

@@ -0,0 +1,11 @@
+fileFormatVersion: 2
+guid: 70508d0b6ef1a4d4db71098b5dd5150b
+MonoImporter:
+  externalObjects: {}
+  serializedVersion: 2
+  defaultReferences: []
+  executionOrder: 0
+  icon: {fileID: 2800000, guid: 936dbb32eb653d84a9bcbe9caad48547, type: 3}
+  userData: 
+  assetBundleName: 
+  assetBundleVariant: 

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();
+        }
+    }
+
+}

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

@@ -0,0 +1,11 @@
+fileFormatVersion: 2
+guid: 1d1367c64c93f1a44bdad6aba236ad5c
+MonoImporter:
+  externalObjects: {}
+  serializedVersion: 2
+  defaultReferences: []
+  executionOrder: 0
+  icon: {fileID: 2800000, guid: 936dbb32eb653d84a9bcbe9caad48547, type: 3}
+  userData: 
+  assetBundleName: 
+  assetBundleVariant: 

Assets/Imstk/Scripts/Interactions/ImstkInteractionBehaviour.cs

@@ -0,0 +1,102 @@
+/*=========================================================================
+
+   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 is the base class of Imstk interaction scripts. It exists to provide
+    /// different init functions for Imstk classes. This is so that we
+    /// can control initialization order in the SimulationManager
+    /// </summary>
+    public class ImstkInteractionBehaviour : MonoBehaviour
+    {
+        // TODO Interaction behavior is not ImstkBehavior ???? 
+        public string GetFullName()
+        {
+            var stack = new System.Collections.Generic.Stack<string>();
+            stack.Push(this.GetType().Name);
+            stack.Push(gameObject.name);
+            Transform parent = gameObject.transform.parent;
+            while (parent != null)
+            {
+                stack.Push(parent.gameObject.name);
+                parent = parent.transform.parent;
+            }
+            var newName = stack.Pop();
+            while (stack.Count != 0)
+            {
+                newName = newName + "/" + stack.Pop();
+            }
+            return newName;
+        }
+        public void ImstkDestroy()
+        {
+            OnImstkDestroy();
+        }
+
+        public virtual Imstk.SceneObject GetImstkInteraction() { return null; }
+
+        public void ImstkInit() { OnImstkInit(); }
+
+        public void ImstkStart() { OnImstkStart(); }
+
+
+        /// <summary>
+        /// Called before initializing the scene
+        /// </summary>
+        protected virtual void OnImstkInit() { }
+
+        /// <summary>
+        /// Called after scene has been initialized
+        /// </summary>
+        protected virtual void OnImstkStart() { }
+
+
+        /// <summary>
+        /// Called when done
+        /// </summary>
+        protected virtual void OnImstkDestroy() { }
+
+        static public (T a, U b) ConvertAndOrder<T, U>(DynamicalModel model1, DynamicalModel model2)
+        where T : Imstk.CollidingObject
+        where U : Imstk.CollidingObject
+            {
+                if (model1 == null || model2 == null)
+                {
+                    Debug.LogError("Interaction input can't be null");
+                }
+
+                if (model1 as T == null && model2 as T == null)
+                {
+                    Debug.LogError("One of the models has to be a " + typeof(T).Name);
+                }
+                if (model1 as U == null && model2 as U == null)
+                {
+                    Debug.LogError("One of the models has to be a " + typeof(U).Name);
+                }
+
+                // This will always return null, null if any of the above errors occur
+                if (model1 as T != null) return (model1 as T, model2 as U);
+                else return (model2 as T, model1 as U);
+            }
+    }
+}

Assets/Imstk/Scripts/Interactions/ImstkInteractionBehaviour.cs.meta

@@ -0,0 +1,11 @@
+fileFormatVersion: 2
+guid: dca4608800d8b1e4dbc91cac64b31bab
+MonoImporter:
+  externalObjects: {}
+  serializedVersion: 2
+  defaultReferences: []
+  executionOrder: 0
+  icon: {fileID: 2800000, guid: 936dbb32eb653d84a9bcbe9caad48547, type: 3}
+  userData: 
+  assetBundleName: 
+  assetBundleVariant: 

Assets/Imstk/Scripts/Interactions/PbdObjectInteraction.cs

@@ -0,0 +1,108 @@
+/*=========================================================================
+
+   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
+{
+    // \todo: Deprecate this
+    [AddComponentMenu("iMSTK/PbdObjectInteraction")]
+    class PbdObjectInteraction : CollisionInteraction
+    {
+        public override Imstk.SceneObject GetImstkInteraction()
+        {
+            if (model1 == null || model2 == null) { 
+                Debug.LogError("Both models need to be set for interaction on " + gameObject.name);
+                return null;
+            }
+
+            string cdTypeName = collisionTypeName.ToString();
+            if (cdTypeName == StandardCollisionTypes.Auto.ToString())
+            {
+                cdTypeName = CollisionInteraction.GetCDType(model1, model2);
+                if (cdTypeName == "")
+                {
+                    return null;
+                }
+            }
+
+            Deformable pbdModel;
+            if (model1 as Deformable != null)
+            {
+                pbdModel = model1 as Deformable;
+            }
+            else
+            {
+                pbdModel = model2 as Deformable;
+                model2 = model1;
+            }
+
+            if (pbdModel == null)
+            {
+                Debug.LogError("One of the DynamicObjects has to be a PbdModel");
+                return null;
+            }
+
+            Imstk.PbdObjectCollision collision = null;
+            if (model2 is StaticModel)
+            {
+                collision =
+                   new Imstk.PbdObjectCollision(
+                       pbdModel.GetDynamicObject() as Imstk.PbdObject,
+                       model2.GetDynamicObject(),
+                       cdTypeName);
+                collision.setDeformableStiffnessA(deformableStiffness1);
+                collision.setDeformableStiffnessB(deformableStiffness2);
+                collision.setRigidBodyCompliance(rigidBodyCompliance);
+            }
+            else if (model2 is Deformable)
+            {
+                collision =
+                    new Imstk.PbdObjectCollision(
+                        pbdModel.GetDynamicObject() as Imstk.PbdObject,
+                        model2.GetDynamicObject() as Imstk.PbdObject,
+                        cdTypeName);
+                collision.setDeformableStiffnessA(deformableStiffness1);
+                collision.setDeformableStiffnessB(deformableStiffness2);
+                collision.setRigidBodyCompliance(rigidBodyCompliance);
+            }
+            else if (model2 is Rigid)
+            {
+                collision =
+                   new Imstk.PbdObjectCollision(
+                       pbdModel.GetDynamicObject() as Imstk.PbdObject,
+                       model2.GetDynamicObject(),
+                       cdTypeName);
+                collision.setDeformableStiffnessA(deformableStiffness1);
+                collision.setDeformableStiffnessB(deformableStiffness2);
+                collision.setRigidBodyCompliance(rigidBodyCompliance);
+            }
+            else
+            {
+                Debug.LogWarning("Could not find interaction for objects " + pbdModel.gameObject + " & " + model2.gameObject);
+                return null;
+            }
+
+            collision.setFriction(friction);
+            collision.setRestitution(restitution);
+            return collision;
+        }
+    }
+}

Assets/Imstk/Scripts/Interactions/PbdObjectInteraction.cs.meta

@@ -0,0 +1,11 @@
+fileFormatVersion: 2
+guid: 9ce368537dc79df418180af2578bf570
+MonoImporter:
+  externalObjects: {}
+  serializedVersion: 2
+  defaultReferences: []
+  executionOrder: 0
+  icon: {fileID: 2800000, guid: 936dbb32eb653d84a9bcbe9caad48547, type: 3}
+  userData: 
+  assetBundleName: 
+  assetBundleVariant: