Update Track mapping with the new ontology

Author: FilipAlg

AGXUnity/IO/OpenPLX/Errors.cs

@@ -22,7 +22,11 @@ public enum AgxUnityOpenPLXErrors
     TraitNotImportable = 15,
     NonPrincipalAxis = 16,
     MultipleDistanceDistortions = 17,
-    NonSymmetricSweepPattern = 18,
+    MissingTrackSystem = 18,
+    DuplicateTrackConnection = 19,
+    InvalidLinkDescription = 20,
+    InsufficientTrackWheels = 21,
+    MissingTrackWheelBody = 22,
   }
 
   public class BaseError : openplx.Error
@@ -281,12 +285,52 @@ public MultipleDistanceDistortionsError( openplx.Core.Object source )
     protected override string createErrorMessage() => $"AGXUnity only supports a single distance distortion";
   }
 
-  public class NonSymmetricSweepPatternError : BaseError
+  public class MissingTrackSystemError : BaseError
   {
-    public NonSymmetricSweepPatternError( openplx.Math.Vec2 FoV )
-      : base( FoV, AgxUnityOpenPLXErrors.NonSymmetricSweepPattern )
+    public MissingTrackSystemError( openplx.Vehicles.TrackSystem.Connections.Base source )
+      : base( source, AgxUnityOpenPLXErrors.MissingTrackSystem )
     { }
 
-    protected override string createErrorMessage() => $"AGXUnity only supports symmetrical sweep patterns";
+    protected override string createErrorMessage() => $"Track connection does not specify a track system";
+  }
+
+  public class DuplicateTrackConnectionError : BaseError
+  {
+    private openplx.Vehicles.TrackSystem.Base m_system;
+
+    public DuplicateTrackConnectionError( openplx.Vehicles.TrackSystem.Connections.Base source, openplx.Vehicles.TrackSystem.Base system )
+      : base( source, AgxUnityOpenPLXErrors.DuplicateTrackConnection )
+    {
+      m_system = system;
+    }
+
+    protected override string createErrorMessage() => $"Track system '{m_system.getName()}' is specified in multiple track connections";
+  }
+
+  public class InvalidLinkDescriptionError : BaseError
+  {
+    public InvalidLinkDescriptionError( openplx.Vehicles.TrackSystem.Components.Tracks.LinkDescription.Base source )
+      : base( source, AgxUnityOpenPLXErrors.InvalidLinkDescription )
+    { }
+
+    protected override string createErrorMessage() => $"Link description needs to have non-zero, positive width and height";
+  }
+
+  public class InsufficientTrackWheelsError : BaseError
+  {
+    public InsufficientTrackWheelsError( openplx.Vehicles.TrackSystem.Base source )
+      : base( source, AgxUnityOpenPLXErrors.InsufficientTrackWheels )
+    { }
+
+    protected override string createErrorMessage() => $"Track system needs to have at least two track wheels";
+  }
+
+  public class MissingTrackWheelBodyError : BaseError
+  {
+    public MissingTrackWheelBodyError( openplx.Vehicles.TrackSystem.Components.TrackWheels.Base source )
+      : base( source, AgxUnityOpenPLXErrors.MissingTrackWheelBody )
+    { }
+
+    protected override string createErrorMessage() => $"Track wheel's body was not properly mapped to a RigidBody";
   }
 }

AGXUnity/IO/OpenPLX/InteractionMapper.cs

@@ -507,6 +507,9 @@ public void MapFrictionModel( ContactMaterial cm, openplx.Physics.Interactions.D
         fm = Data.DefaultFriction;
       }
 
+      if ( friction.HasTrait<openplx.Vehicles.TrackSystem.SurfaceContact.Traits.TrackFriction>() )
+        fm.TrackFrictionModel = true;
+
       var solveType = FrictionModel.ESolveType.Direct;
       var solveTypeAnnotation = friction.findAnnotations("agx_friction_solve_type");
       var approximateConeAnnotation = friction.findAnnotations("agx_approximate_cone_friction");

AGXUnity/IO/OpenPLX/MappingUtils.cs

@@ -74,6 +74,58 @@ public static bool HasTrait<T>( this openplx.Core.Object obj )
       string traitName = typeof( T ).FullName.Substring( 8 );
       return obj.hasTrait( traitName );
     }
+
+    public static bool TryGetBoolAnnotation( this openplx.Core.Object obj, string name, out bool value )
+    {
+      value = true; // This will be overwritten
+      var annots = obj.findAnnotations(name);
+      if ( annots.IsEmpty )
+        return false;
+
+      foreach ( var annot in annots ) {
+        if ( annot.isTrue() ) {
+          value = true;
+          return true;
+        }
+        if ( annot.isFalse() ) {
+          value = false;
+          return true;
+        }
+      }
+      return false;
+    }
+
+    public static bool TryGetRealAnnotation( this openplx.Core.Object obj, string name, out float value )
+    {
+      value = 0.0f; // This will be overwritten
+      var annots = obj.findAnnotations(name);
+      if ( annots.IsEmpty )
+        return false;
+
+      foreach ( var annot in annots ) {
+        if ( annot.isNumber() ) {
+          value = (float)annot.asReal();
+          return true;
+        }
+      }
+      return false;
+    }
+
+    public static bool TryGetStringAnnotation( this openplx.Core.Object obj, string name, out string value )
+    {
+      value = ""; // This will be overwritten
+      var annots = obj.findAnnotations(name);
+      if ( annots.IsEmpty )
+        return false;
+
+      foreach ( var annot in annots ) {
+        if ( annot.isString() ) {
+          value = annot.asString();
+          return true;
+        }
+      }
+      return false;
+    }
   }
   public static class Utils
   {
@@ -98,6 +150,11 @@ public static T ReportUnimplemented<T>( openplx.Core.Object obj, openplx.ErrorRe
       return null;
     }
 
+    public static void ReportUnimplemented( openplx.Core.Object obj, openplx.ErrorReporter err )
+    {
+      err.reportError( new UnimplementedError( obj ) );
+    }
+
     public static void AddChild( GameObject parent, GameObject child, openplx.ErrorReporter err, openplx.Core.Object obj )
     {
       if ( child != null )

AGXUnity/IO/OpenPLX/OpenPLXRoot.cs

@@ -115,24 +115,26 @@ protected override bool Initialize()
       var errorReporter = new ErrorReporter();
       var mapper = new agxopenplx.OpenPlxDriveTrainMapper( errorReporter, map );
 
-      agxPowerLine.PowerLineRef powerline = new agxPowerLine.PowerLineRef(new agxPowerLine.PowerLine());
-      mapper.mapDriveTrainIntoPowerLine( Native as openplx.Physics.System, powerline );
-
       RuntimeMapped = new Dictionary<string, agx.Referenced>();
-      Simulation.Instance.Native.add( powerline.get() );
-      foreach ( var (obj, constraint) in mapper.getMappedConstraints() ) {
-        Simulation.Instance.Native.add( constraint.get() );
-        RuntimeMapped.Add( obj.getName(), constraint.get() );
-      }
-      foreach ( var obj in powerline.getUnits() ) {
-        var objName = obj.getName();
-        if ( objName.StartsWith( PrunedNativeName + "." ) )
-          RuntimeMapped.Add( objName, obj.get() );
-      }
-      foreach ( var obj in powerline.getConnectors() ) {
-        var objName = obj.getName();
-        if ( objName.StartsWith( PrunedNativeName + "." ) )
-          RuntimeMapped.Add( objName, obj.get() );
+
+      agxPowerLine.PowerLineRef powerline = mapper.mapDriveTrainIntoPowerLine( Native as openplx.Physics.System);
+      // TODO: Fix null return from this method
+      if ( powerline != null && powerline.get() != null ) {
+        Simulation.Instance.Native.add( powerline.get() );
+        foreach ( var (obj, constraint) in mapper.getMappedConstraints() ) {
+          Simulation.Instance.Native.add( constraint.get() );
+          RuntimeMapped.Add( obj.getName(), constraint.get() );
+        }
+        foreach ( var obj in powerline.getUnits() ) {
+          var objName = obj.getName();
+          if ( objName.StartsWith( PrunedNativeName + "." ) )
+            RuntimeMapped.Add( objName, obj.get() );
+        }
+        foreach ( var obj in powerline.getConnectors() ) {
+          var objName = obj.getName();
+          if ( objName.StartsWith( PrunedNativeName + "." ) )
+            RuntimeMapped.Add( objName, obj.get() );
+        }
       }
 
       return base.Initialize();

AGXUnity/IO/OpenPLX/OpenPLXUnityMapper.cs

@@ -49,7 +49,7 @@ public class OpenPLXUnityMapper
       Options = options;
 
       InteractionMapper = new InteractionMapper( Data );
-      VehicleMapper = new VehicleMapper( Data );
+      VehicleMapper = new VehicleMapper( Data, InteractionMapper );
       SensorMapper = new SensorMapper( Data );
     }
 
@@ -851,6 +851,8 @@ GameObject MapSystem( openplx.Physics3D.System system )
     {
       var s = MapSystemPass1( system );
       MapSystemPass2( system );
+      // This method performs actual mapping of previously discovered tracks and is not recursive
+      VehicleMapper.MapTrackSystems();
       MapSystemPass3( system );
       MapSystemPass4( system );
       MapSystemPass5( system );
@@ -884,14 +886,8 @@ GameObject MapSystemPass1( openplx.Physics3D.System system )
             Data.MaterialCache[ geometry.material() ] = Data.DefaultMaterial;
         }
       }
-      foreach ( var trackSystem in system.getNonReferenceValues<openplx.Vehicles.Tracks.System>() ) {
-        if ( trackSystem.belt().link_description() is openplx.Vehicles.Tracks.BoxLinkDescription desc ) {
-          var mat = desc.contact_geometry().material();
-          if ( !Data.MaterialCache.ContainsKey( mat ) ) {
-            Data.MaterialCache[ mat ] = InteractionMapper.MapMaterial( mat );
-          }
-        }
-      }
+
+      VehicleMapper.MapSystemPass1( system );
 
       return s;
     }
@@ -919,6 +915,8 @@ void MapSystemPass2( openplx.Physics3D.System system )
 
       foreach ( var terr in system.getNonReferenceValues<openplx.Terrain.Terrain>() )
         Utils.AddChild( s, MapTerrain( terr ), Data.ErrorReporter, terr );
+
+      VehicleMapper.MapSystemPass2( system );
     }
 
     void MapSystemPass3( openplx.Physics3D.System system )
@@ -928,9 +926,6 @@ void MapSystemPass3( openplx.Physics3D.System system )
       foreach ( var subSystem in system.getNonReferenceValues<openplx.Physics3D.System>() )
         MapSystemPass3( subSystem );
 
-      foreach ( var trackSystem in system.getNonReferenceValues<openplx.Vehicles.Tracks.System>() )
-        VehicleMapper.MapTrackSystem( trackSystem );
-
       foreach ( var mateConnector in system.getNonReferenceValues<openplx.Physics3D.Interactions.MateConnector>() )
         InteractionMapper.MapMateConnector( mateConnector );
 
@@ -946,8 +941,7 @@ void MapSystemPass4( openplx.Physics3D.System system )
       foreach ( var subSystem in system.getNonReferenceValues<openplx.Physics3D.System>() )
         MapSystemPass4( subSystem );
 
-      if ( system is openplx.Vehicles.Suspensions.SingleMate.Base suspension )
-        VehicleMapper.MapSingleMateSuspensionOnto( suspension, s );
+      VehicleMapper.MapSystemPass4( system );
 
       foreach ( var lidar in system.getNonReferenceValues<openplx.Sensors.LidarLogic>() )
         Utils.AddChild( s, SensorMapper.MapLidar( lidar ), Data.ErrorReporter, lidar );
@@ -985,11 +979,7 @@ void MapSystemPass5( openplx.Physics3D.System system )
       foreach ( var subSystem in system.getNonReferenceValues<openplx.Physics3D.System>() )
         MapSystemPass5( subSystem );
 
-      if ( system is openplx.Vehicles.Steering.Kinematic.Base steering )
-        VehicleMapper.MapSteeringOnto( steering, s );
-
-      foreach ( var wheel in system.getNonReferenceValues<openplx.Vehicles.Wheels.ElasticWheel>() )
-        VehicleMapper.MapElasticWheel( wheel );
+      VehicleMapper.MapSystemPass5( system );
     }
   }
 }