Merge pull request #173 from Algoryx/feature/cable-tunneling-guard

Cable Tunneling Guard

.editorconfig

@@ -5,6 +5,7 @@ root = true
 [*.cs]
 
 dotnet_diagnostic.IDE0005.severity = error
+dotnet_diagnostic.IDE0031.severity = none
 
 #### Core EditorConfig Options ####
 

AGXUnity/CableTunnelingGuard.cs

@@ -0,0 +1,248 @@
+using AGXUnity.Utils;
+using System.Collections.Generic;
+using System.Linq;
+using UnityEngine;
+using UnityEngine.UIElements;
+
+namespace AGXUnity
+{
+  [AddComponentMenu( "AGXUnity/Cable Tunneling Guard" )]
+  [DisallowMultipleComponent]
+  [RequireComponent( typeof( AGXUnity.Cable ) )]
+  [HelpURL( "https://us.download.algoryx.se/AGXUnity/documentation/current/editor_interface.html#cable-tunneling-guard" )]
+  public class CableTunnelingGuard : ScriptComponent
+  {
+    /// <summary>
+    /// Native instance of the cable tuneling guard.
+    /// </summary>
+    public agxCable.CableTunnelingGuard Native { get; private set; }
+
+    [System.NonSerialized]
+    private Cable m_cable = null;
+
+    /// <summary>
+    /// The Cable ScriptComponent that this CableTunnelingGuard follows
+    /// </summary>
+    [HideInInspector]
+    public Cable Cable { get { return m_cable ??= GetComponent<Cable>(); } }
+
+    /// <summary>
+    /// The mesh which is used to visualise a hull
+    /// </summary>
+    private Mesh m_mesh = null;
+
+    [SerializeField]
+    private double m_hullScale = 4;
+
+    public double HullScale
+    {
+      get { return m_hullScale; }
+      set
+      {
+        value = System.Math.Max( value, 1.0f );
+
+        if ( m_hullScale != value ) {
+          m_hullScale = value;
+          UpdateRenderingMesh();
+        }
+
+        if ( Native != null ) {
+          Native.setHullScale( m_hullScale );
+        }
+      }
+    }
+
+    private void UpdateRenderingMesh()
+    {
+      if ( m_mesh == null )
+        m_mesh = new Mesh();
+
+      if ( m_pointCurveCache != null && m_pointCurveCache.Length >= 2 ) {
+        float segmentLength = ( Cable.GetRoutePoints()[ 0 ]-Cable.GetRoutePoints()[ 1 ] ).magnitude;
+        CapsuleShapeUtils.CreateCapsuleMesh( Cable.Radius * (float)m_hullScale, segmentLength, 0.7f, m_mesh );
+      }
+    }
+
+    // See documentation / tutorials for a more detailed description of the native parameters
+
+    /// <summary>
+    /// The angle to cable ends at which and approaching contact is accepted
+    /// </summary>
+    [SerializeField]
+    private double m_angleThreshold = 90.0 * 0.9;
+
+    public double AngleThreshold
+    {
+      get { return m_angleThreshold; }
+      set
+      {
+        m_angleThreshold = System.Math.Clamp( value, 0, 90 );
+        if ( Native != null ) {
+          Native.setAngleThreshold( m_angleThreshold / 180.0 * Mathf.PI );
+        }
+      }
+    }
+
+    /// <summary>
+    /// A parameter which controls how far the estimated penetration depth must be at the enxt step to attempt to
+    /// prevent a tunneling occurence
+    /// </summary>
+    [SerializeField]
+    private double m_leniency = 0;
+
+    public double Leniency
+    {
+      get { return m_leniency; }
+      set
+      {
+        m_leniency = value;
+        if ( Native != null ) {
+          Native.setLeniency( m_leniency );
+        }
+      }
+    }
+
+    /// <summary>
+    /// The amount of steps for which the component will continue adding contacts to the solver after a contact has
+    /// been predicted.
+    /// </summary>
+    [SerializeField]
+    private uint m_debounceSteps = 0;
+
+    public uint DebounceSteps
+    {
+      get { return m_debounceSteps; }
+      set
+      {
+        m_debounceSteps = value;
+        if ( Native != null ) {
+          Native.setDebounceSteps( m_debounceSteps );
+        }
+      }
+    }
+
+    /// <summary>
+    /// When set to true the component will not attempt any predictions and will always add the contacts it encounters 
+    /// through the hulls to the solver
+    /// </summary>
+    [SerializeField]
+    private bool m_alwaysAdd = false;
+
+    public bool AlwaysAdd
+    {
+      get { return m_alwaysAdd; }
+      set
+      {
+        m_alwaysAdd = value;
+        if ( Native != null ) {
+          Native.setAlwaysAdd( m_alwaysAdd );
+        }
+      }
+    }
+
+    /// <summary>
+    /// When true the component will predict tunneling with its own segments as well
+    /// </summary>
+    [SerializeField]
+    private bool m_enableSelfInteraction = true;
+
+    public bool EnableSelfInteraction
+    {
+      get { return m_enableSelfInteraction; }
+      set
+      {
+        m_enableSelfInteraction = value;
+        if ( Native != null ) {
+          Native.setEnableSelfInteraction( m_enableSelfInteraction );
+        }
+      }
+    }
+
+    protected override bool Initialize()
+    {
+      Native = new agxCable.CableTunnelingGuard( m_hullScale );
+
+      var cable = Cable?.GetInitialized<Cable>()?.Native;
+      if ( cable == null ) {
+        Debug.LogWarning( "Unable to find Cable component for CableTunnelingGuard - cable tunneling guard instance ignored.", this );
+        return false;
+      }
+
+      cable.addComponent( Native );
+
+      return true;
+    }
+
+    protected override void OnDestroy()
+    {
+      if ( GetSimulation() == null )
+        return;
+
+      var cable = Cable.Native;
+      if ( cable != null ) {
+        cable.removeComponent( Native );
+      }
+
+      Native = null;
+
+      base.OnDestroy();
+    }
+
+    protected override void OnEnable()
+    {
+      Native?.setEnabled( true );
+    }
+
+    protected override void OnDisable()
+    {
+      Native?.setEnabled( false );
+    }
+
+    private void Reset()
+    {
+      if ( GetComponent<Cable>() == null )
+        Debug.LogError( "Component: CableDamage requires Cable component.", this );
+    }
+
+    private Vector3[] m_pointCurveCache = null;
+
+    private bool CheckCableRouteChanges()
+    {
+      var routePointCahce = Cable.GetRoutePoints();
+      if ( m_pointCurveCache != routePointCahce ) {
+        m_pointCurveCache = routePointCahce;
+        return true;
+      }
+      return false;
+    }
+
+    private void OnDrawGizmosSelected()
+    {
+      if ( CheckCableRouteChanges() ) {
+        UpdateRenderingMesh();
+      }
+
+      if ( enabled ) {
+        // Algoryx orange
+        Gizmos.color = new Color32( 0xF3, 0x8B, 0x00, 0xF );
+        if ( Application.isPlaying && Cable?.Native != null ) {
+          foreach ( var segment in Cable.Native.getSegments() ) {
+            Vector3 direction = (segment.getEndPosition() - segment.getBeginPosition()).ToHandedVector3();
+            Vector3 center = segment.getCenterPosition().ToHandedVector3();
+            Gizmos.DrawWireMesh( m_mesh, center, Quaternion.FromToRotation( Vector3.up, direction ) );
+          }
+        }
+        else if ( m_pointCurveCache != null && m_pointCurveCache.Length != 0 ) {
+          Vector3 prevPoint = m_pointCurveCache[0];
+          foreach ( var point in m_pointCurveCache.Skip( 1 ) ) {
+            Vector3 direction = point-prevPoint;
+            Vector3 center = prevPoint + direction * 0.5f;
+            Gizmos.DrawWireMesh( m_mesh, center, Quaternion.FromToRotation( Vector3.up, direction ) );
+            prevPoint = point;
+          }
+        }
+      }
+    }
+  }
+
+}

AGXUnity/Rendering/ShapeDebugRenderData.cs

@@ -223,6 +223,7 @@ private bool TryInitialize( Shape shape, DebugRenderManager manager )
       Cone cone                     = shape as Cone;
       HollowCone hollowCone         = shape as HollowCone;
       HollowCylinder hollowCylinder = shape as HollowCylinder;
+      Capsule capsule               = shape as Capsule;
       if ( mesh != null )
         Node = InitializeMesh( mesh );
       else if ( heightField != null )
@@ -242,6 +243,11 @@ private bool TryInitialize( Shape shape, DebugRenderManager manager )
         Node.AddComponent<MeshRenderer>().sharedMaterial = manager.ShapeRenderMaterial;
         Node.AddComponent<MeshFilter>().sharedMesh = ShapeVisualCone.GenerateMesh( shape );
       }
+      else if ( capsule != null ) {
+        Node = new GameObject( PrefabName );
+        Node.AddComponent<MeshRenderer>().sharedMaterial = manager.ShapeRenderMaterial;
+        Node.AddComponent<MeshFilter>().sharedMesh = ShapeVisualCapsule.GenerateMesh( shape );
+      }
       else {
         Node = PrefabLoader.Instantiate<GameObject>( PrefabName );
         Node.transform.localScale = GetShape().GetScale();

AGXUnity/Rendering/ShapeVisual.cs

@@ -400,7 +400,7 @@ protected static GameObject CreateGameObject( Collide.Shape shape, bool isRender
     {
       GameObject go = null;
       try {
-        go = isRenderData || shape is Collide.Mesh || shape is Collide.HollowCylinder || shape is Collide.Cone || shape is Collide.HollowCone ?
+        go = isRenderData || shape is Collide.Mesh || shape is Collide.HollowCylinder || shape is Collide.Cone || shape is Collide.HollowCone || shape is Collide.Capsule ?
                new GameObject( "" ) :
                PrefabLoader.Instantiate<GameObject>( @"Debug/" + shape.GetType().Name + "Renderer" );
 

AGXUnity/Rendering/ShapeVisualCapsule.cs

@@ -1,4 +1,6 @@
-using UnityEngine;
+using AGXUnity.Utils;
+using System.Linq;
+using UnityEngine;
 
 namespace AGXUnity.Rendering
 {
@@ -10,15 +12,40 @@ namespace AGXUnity.Rendering
   [HelpURL( "https://us.download.algoryx.se/AGXUnity/documentation/current/editor_interface.html#create-visual-tool-icon-small-create-visual-tool" )]
   public class ShapeVisualCapsule : ShapeVisual
   {
+    private Mesh m_mesh = null;
+
+    private const float m_resolution = 1;
+
+    /// <summary>
+    /// Callback when constructed.
+    /// </summary>
+    protected override void OnConstruct()
+    {
+      gameObject.AddComponent<MeshFilter>();
+      gameObject.AddComponent<MeshRenderer>();
+
+      m_mesh = GenerateMesh( Shape );
+      gameObject.GetComponent<MeshFilter>().sharedMesh = m_mesh;
+    }
+
     /// <summary>
-    /// Capsule visual is three game objects (2 x half sphere + 1 x cylinder),
-    /// the size has to be updated to all of the children.
+    /// Callback from Shape when its size has been changed.
     /// </summary>
     public override void OnSizeUpdated()
     {
-      ShapeDebugRenderData.SetCapsuleSize( gameObject,
-                                           ( Shape as Collide.Capsule ).Radius,
-                                           ( Shape as Collide.Capsule ).Height );
+      transform.localScale = GetUnscaledScale();
+
+      m_mesh = GenerateMesh( Shape );
+      gameObject.GetComponent<MeshFilter>().sharedMesh = m_mesh;
+    }
+
+    /// <summary>
+    /// Generates custom shape mesh
+    /// </summary>
+    public static Mesh GenerateMesh( Collide.Shape shape )
+    {
+      Collide.Capsule capsule = shape as Collide.Capsule;
+      return CapsuleShapeUtils.CreateCapsuleMesh( capsule.Radius, capsule.Height, m_resolution );
     }
   }
 }

AGXUnity/Utils/ShapeUtils.cs

@@ -1,4 +1,5 @@
 using AGXUnity.Collide;
+using System.Linq;
 using UnityEngine;
 
 namespace AGXUnity.Utils
@@ -44,6 +45,31 @@ public override Vector3 GetLocalFace( Direction dir )
       else
         return capsule.Radius * GetLocalFaceDirection( dir );
     }
+
+    public static UnityEngine.Mesh CreateCapsuleMesh( float radius, float height, float resolution, UnityEngine.Mesh destination = null )
+    {
+      destination ??= new UnityEngine.Mesh();
+
+      radius = Mathf.Max( 0, radius );
+      height = Mathf.Max( 0, height );
+
+      agxCollide.MeshData meshData = agxUtil.PrimitiveMeshGenerator.createCapsule( radius, height, resolution ).getMeshData();
+      var agxIndices = meshData.getIndices();
+      int[] triangles = new int[agxIndices.Count];
+
+      // Flip winding order
+      for ( int i = 0; i < triangles.Length; i+=3 )
+        (triangles[ i ], triangles[ i+2 ], triangles[ i+1 ]) = ((int)agxIndices[ i ], (int)agxIndices[ i+1 ], (int)agxIndices[ i+2 ]);
+
+
+      destination.SetVertices( meshData.getVertices().Select( v => v.ToHandedVector3() ).ToArray() );
+      destination.SetTriangles( triangles, 0, calculateBounds: false );
+
+      destination.RecalculateNormals();
+      destination.RecalculateTangents();
+
+      return destination;
+    }
   }
 
   public class CylinderShapeUtils : RadiusHeightShapeUtils<Cylinder>

Editor/AGXUnityEditor/InspectorEditor.cs

@@ -323,7 +323,7 @@ private static bool HandleType( InvokeWrapper wrapper,
             Debug.LogWarning( "The AGXUnity Inspector wrapper currently does not support editable array types. Consider using List<T> as an alternative." );
           if ( EditorGUI.EndChangeCheck() && assignSupported ) {
             changed = true;
-            value = serializedProperty.objectReferenceValue;
+            value = serializedProperty.boxedValue;
           }
         }
       }