public static void Create(Transform parent, GeometryTypes type, Transform visualToCopy = null)
{
GameObject collisionObject = new GameObject("unnamed");
collisionObject.transform.SetParentAndAlign(parent);
UrdfCollision urdfCollision = collisionObject.AddComponent <UrdfCollision>();
urdfCollision.geometryType = type;
if (visualToCopy != null)
{
if (urdfCollision.geometryType == GeometryTypes.Mesh)
{
UrdfGeometryCollision.CreateMatchingMeshCollision(collisionObject.transform, visualToCopy);
}
else
{
UrdfGeometryCollision.Create(collisionObject.transform, type);
}
//copy transform values from corresponding UrdfVisual
collisionObject.transform.position = visualToCopy.position;
collisionObject.transform.localScale = visualToCopy.localScale;
collisionObject.transform.rotation = visualToCopy.rotation;
}
else
{
UrdfGeometryCollision.Create(collisionObject.transform, type);
}
UnityEditor.EditorGUIUtility.PingObject(collisionObject);
}
/// <summary>
/// 3D UZ !!!!
/// Creates a dictionary with key equal to the model state index and the value the spatial information of that state index.
/// </summary>
/// <param name="gType">The geometric type of the exchange itme (2d or 3d)</param>
/// <param name="baseOut">The exchange item base output</param>
/// <param name="elementID">the string id of the exchange item.</param>
/// <returns></returns>
private IDictionary <int, ISpatialDefine> GetModelCoordinates3DUZ(GeometryTypes gType, IBaseOutput baseOut, string elementID)
{
IDictionary <int, ISpatialDefine> modelEntities = new Dictionary <int, ISpatialDefine>();
int n;
try
{
WMEngine.GetElementCount(elementID);
n = baseOut.ElementSet().ElementCount;
}
catch
{
Console.WriteLine("\nElement {0} does not found in the model\n", elementID);
throw new Exception("\nProblem in Model Instance - unable to find exchange item\n");
}
// Determines the number of layers in the UZ Grid.
int numLayersInGrid = Convert.ToInt32(Math.Round((double)base.WMEngine.UzGrid.ElementCount / (double)base.WMEngine.UzGrid.BaseGrid.ElementCount));
for (int i = 0; i < n; i++)
{
XYPolygon modelpolygon = ElementMapper.CreateXYPolygon(baseOut.ElementSet(), i);
int zLayer = Convert.ToInt32(i % numLayersInGrid);
// Points in Polygon are defined as LL, LR, UR, UL (l/l = lower/left, u = upper, r = right )
// Finds the mid x and mid y point in the polygon (assuming rectangular grid)
IXYLayerPoint min = new XYLayerPoint(modelpolygon.GetX(0), modelpolygon.GetY(0), zLayer);
IXYLayerPoint max = new XYLayerPoint(modelpolygon.GetX(1), modelpolygon.GetY(3), zLayer);
modelEntities.Add(i, new SpatialDefine(min, max, GeometryTypes.Geometry3DUZ));
}
return(modelEntities);
}
public static string GetGeometryTypeName(this GeometryTypes geometryType)
{
switch (geometryType)
{
case GeometryTypes.Point:
{
return(POINT_GEOMETRY);
}
case GeometryTypes.Multipoint:
{
return(MULTIPOINT_GEOMETRY);
}
case GeometryTypes.Polyline:
{
return(POLYLINE_GEOMETRY);
}
case GeometryTypes.Polygon:
{
return(POLYGON_GEOMETRY);
}
case GeometryTypes.Envelope:
{
return(ENVELOPE_GEOMETRY);
}
default:
{
return(null);
}
}
}
/// <summary>
/// Get vertex count of specified geometry's type.
/// <para>returns -1 if type is <see cref="Geometry.Polygon"/>.</para>
/// </summary>
/// <param name="type"></param>
/// <returns></returns>
public static int GetVertexCount(this GeometryTypes type)
{
int result = -1;
switch (type)
{
case GeometryTypes.Point:
result = 1;
break;
case GeometryTypes.Line:
result = 2;
break;
case GeometryTypes.Triangle:
result = 3;
break;
case GeometryTypes.Quad:
result = 4;
break;
case GeometryTypes.Polygon:
result = -1;
break;
default:
throw new NotImplementedException();
}
return(result);
}
public static void SetScale(Transform transform, Link.Geometry geometry, GeometryTypes geometryType)
{
switch (geometryType)
{
case GeometryTypes.Box:
transform.localScale =
new Vector3((float)geometry.box.size[1], (float)geometry.box.size[2], (float)geometry.box.size[0]);
break;
case GeometryTypes.Cylinder:
transform.localScale = new Vector3(
(float)geometry.cylinder.radius * 2,
(float)geometry.cylinder.length / 2,
(float)geometry.cylinder.radius * 2);
break;
case GeometryTypes.Sphere:
transform.localScale = new Vector3(
(float)geometry.sphere.radius * 2,
(float)geometry.sphere.radius * 2,
(float)geometry.sphere.radius * 2);
break;
case GeometryTypes.Mesh:
if (geometry?.mesh?.scale != null)
{
Vector3 scale = geometry.mesh.scale.ToVector3().Ros2UnityScale();
transform.localScale = Vector3.Scale(transform.localScale, scale);
transform.localPosition = Vector3.Scale(transform.localPosition, scale);
}
break;
}
}
/// <summary>
/// Returns a List of ISpatialDefine with assumed 90deg angles.
/// The ISpatialDefine is defined by two coordinates and a layer integer.
/// A ISpatialDefine can represent a cuboid, rectangle or a point.
/// </summary>
/// <param name="elementID"></param>
/// <returns></returns>
private IDictionary <int, ISpatialDefine> GetModelCoordinates(string elementIDAndGrid)
{
char[] delimiterChars = { ',' };
string[] words = elementIDAndGrid.Split(delimiterChars);
string elementID = words[0].Trim();
IBaseOutput baseOut = base._outputExchangeItems.First(vID => string.Compare(vID.Id, elementID) == 0);
// Get the Grid Type from the elementID (delimiated by a ',');
GeometryTypes gType = GetGridType(elementID);
if (gType == GeometryTypes.Geometry3DSZ)
{
return(GetModelCoordinates3DSZ(gType, baseOut, elementID));
}
if (gType == GeometryTypes.Geometry3DUZ)
{
return(GetModelCoordinates3DUZ(gType, baseOut, elementID));
}
if (gType == GeometryTypes.Geometry2D)
{
return(GetModelCoordinates2D(gType, baseOut, elementID));
}
else
{
throw new Exception("The Rest not Implemented");
}
}
public static Link.Geometry ExportGeometryData(GeometryTypes geometryType, Transform transform, bool isCollisionGeometry = false)
{
Link.Geometry geometry = null;
switch (geometryType)
{
case GeometryTypes.Box:
geometry = new Link.Geometry(new Link.Geometry.Box(ExportUrdfSize(transform)));
break;
case GeometryTypes.Cylinder:
geometry = new Link.Geometry(
null,
new Link.Geometry.Cylinder(ExportUrdfRadius(transform), ExportCylinderHeight(transform)));
break;
case GeometryTypes.Sphere:
geometry = new Link.Geometry(null, null, new Link.Geometry.Sphere(ExportUrdfRadius(transform)));
break;
case GeometryTypes.Mesh:
geometry = ExportGeometryMeshData(transform.GetChild(0).gameObject, ExportUrdfSize(transform), isCollisionGeometry);
break;
}
return(geometry);
}
public void IsTransformed_Default_False(Geometry geometry, Vector3 scale, GeometryTypes type)
{
var parent = new GameObject("Parent").transform;
Assert.IsFalse(UrdfGeometry.IsTransformed(parent, UrdfGeometry.GetGeometryType(geometry)));
Object.DestroyImmediate(parent.gameObject);
}
public void IsTransformed_Rotation_True(Geometry geometry, Vector3 scale, GeometryTypes type)
{
var parent = new GameObject("Parent").transform;
parent.localRotation = Quaternion.Euler(0, 30, 0);
Assert.IsTrue(UrdfGeometry.IsTransformed(parent, UrdfGeometry.GetGeometryType(geometry)));
Object.DestroyImmediate(parent.gameObject);
}
public void IsTransformed_Scale_True(Geometry geometry, Vector3 scale, GeometryTypes type)
{
var parent = new GameObject("Parent").transform;
parent.localScale = Vector3.one * 2f;
Assert.IsTrue(UrdfGeometry.IsTransformed(parent, UrdfGeometry.GetGeometryType(geometry)));
Object.DestroyImmediate(parent.gameObject);
}
public Block(int definedWidth, int definedColumn, int definedRow, GeometryTypes GeomTypeDefined, String color)
{
GeomType = GeomTypeDefined;
Width = definedWidth;
Color = color;
Row = definedRow;
Column = definedColumn;
setPathGeometry();
}
public static void Create(Transform parent, GeometryTypes type)
{
GameObject visualObject = new GameObject("unnamed");
visualObject.transform.SetParentAndAlign(parent);
UrdfVisual urdfVisual = visualObject.AddComponent <UrdfVisual>();
urdfVisual.GeometryType = type;
UrdfGeometryVisual.Create(visualObject.transform, type);
}
/// <summary>
/// Constructor 2D or 3D
/// </summary>
public SpatialDefine(IXYLayerPoint pt1, IXYLayerPoint pt2, GeometryTypes gm)
{
_pt1 = pt1;
_pt2 = pt2;
_geometry = gm;
if (pt1.Layer != pt2.Layer)
{
throw new Exception("Layer number must be the same for the two points");
}
}
public override void OnInspectorGUI()
{
urdfCollisions = (UrdfCollisions)target;
GUILayout.Space(10);
geometryType = (GeometryTypes)EditorGUILayout.EnumPopup("Type of collision", geometryType);
if (GUILayout.Button("Add collision"))
{
UrdfCollisionExtensions.Create(urdfCollisions.transform, geometryType);
}
}
public override void OnInspectorGUI()
{
GUILayout.Space(10);
geometryType = (GeometryTypes)EditorGUILayout.EnumPopup("Type of visual", geometryType);
EditorGUILayout.BeginHorizontal();
if (GUILayout.Button("Add visual"))
{
UrdfVisualExtensions.Create(urdfVisuals.transform, geometryType);
}
EditorGUILayout.EndHorizontal();
}
/// <summary>
/// OpenMI does not know about localization. In this method, the user can implement localization for OpenMI models in this method
/// </summary>
/// <param name="exchangeItemId">the exchange ID string</param>
/// <param name="observationDescriptions">OpenDA type of observation description</param>
/// <param name="locDistance">the distance for gaussian localization</param>
/// <returns></returns>
public double[][] getLocalization(string exchangeItemId, OpenDA.DotNet.Interfaces.IObservationDescriptions observationDescriptions, double locDistance)
{
//Get the Keys from the observer
String[] keys = observationDescriptions.PropertyKeys;
String[] quantity = observationDescriptions.GetStringProperties("quantity");
double[] xpos = observationDescriptions.GetValueProperties("xposition").Values;
double[] ypos = observationDescriptions.GetValueProperties("yposition").Values;
double[] height = observationDescriptions.GetValueProperties("height").Values;
int observationCount = observationDescriptions.ObservationCount;
// The heights should be specified in an array of integers representing the layer. Check if the values are indeed integers or close to integers before converting
// the array of doubles to an array of integers.
for (int i = 0; i < height.Length; i++)
{
if (double.IsNaN(height[i]))
{
height[i] = 0.0;
}
}
const double tolerance = 1e-5;
int[] layer = new int[observationCount];
for (int i = 0; i < observationCount; i++)
{
layer[i] = Convert.ToInt32(height[i]);
if (Math.Abs(layer[i] - height[i]) > tolerance)
{
throw new Exception("The height specified in the observation was not an integer. Observation \n");
}
}
// Gets the Grid type for the model. Can be a number of possibilities depending on the variable.
GeometryTypes geometrytype = GetGridType(exchangeItemId);
//BaseGrid
if (geometrytype == GeometryTypes.Geometry2D)
{
return(GetLocalized2D(exchangeItemId, observationCount, locDistance, xpos, ypos));
}
if (geometrytype == GeometryTypes.Geometry3DSZ)
{
double[][] mask = GetLocalized3DSZ(exchangeItemId, observationCount, locDistance, xpos, ypos, layer);
return(mask);
}
else
{
throw new NotImplementedException("Only 3D SZ and 2D BaseGrid supported so far.");
}
}
/// <summary>
/// Convert <see cref="BeginMode"/> to <see cref="GeometryTypes"/>.
/// </summary>
/// <param name="mode"></param>
/// <returns></returns>
public static GeometryTypes ToGeometryType(this PrimitiveMode mode)
{
GeometryTypes result = GeometryTypes.Point;
switch (mode)
{
case PrimitiveMode.Points:
result = GeometryTypes.Point;
break;
case PrimitiveMode.Lines:
result = GeometryTypes.Line;
break;
case PrimitiveMode.LineLoop:
result = GeometryTypes.Line;
break;
case PrimitiveMode.LineStrip:
result = GeometryTypes.Line;
break;
case PrimitiveMode.Triangles:
result = GeometryTypes.Triangle;
break;
case PrimitiveMode.TriangleStrip:
result = GeometryTypes.Triangle;
break;
case PrimitiveMode.TriangleFan:
result = GeometryTypes.Triangle;
break;
case PrimitiveMode.Quads:
result = GeometryTypes.Quad;
break;
case PrimitiveMode.QuadStrip:
result = GeometryTypes.Quad;
break;
case PrimitiveMode.Polygon:
result = GeometryTypes.Polygon;
break;
default:
throw new NotImplementedException();
}
return(result);
}
public static void Create(Transform parent, GeometryTypes type)
{
GameObject visualObject = new GameObject("unnamed");
visualObject.transform.SetParentAndAlign(parent);
UrdfVisual urdfVisual = visualObject.AddComponent <UrdfVisual>();
urdfVisual.geometryType = type;
UrdfGeometryVisual.Create(visualObject.transform, type);
#if UNITY_EDITOR
UnityEditor.EditorGUIUtility.PingObject(visualObject);
#endif
}
public static void CheckForUrdfCompatibility(Transform transform, GeometryTypes type)
{
Transform childTransform = transform.GetChild(0);
if (IsTransformed(childTransform, type))
{
Debug.LogWarning("Changes to the transform of " + childTransform.name + " cannot be exported to URDF. " +
"Make any translation, rotation, or scale changes to the parent Visual or Collision object instead.",
childTransform);
}
if (!transform.HasExactlyOneChild())
{
Debug.LogWarning("Only one Geometry element is allowed for each Visual or Collision element. In "
+ transform.parent.parent.name + ", move each Geometry into its own Visual or Collision.", transform);
}
}
public static void Create(Transform parent, GeometryTypes geometryType, Link.Geometry geometry = null)
{
GameObject geometryGameObject = null;
switch (geometryType)
{
case GeometryTypes.Box:
geometryGameObject = new GameObject(geometryType.ToString());
geometryGameObject.AddComponent <BoxCollider>();
break;
case GeometryTypes.Cylinder:
geometryGameObject = CreateCylinderCollider();
break;
case GeometryTypes.Sphere:
geometryGameObject = new GameObject(geometryType.ToString());
geometryGameObject.AddComponent <SphereCollider>();
break;
case GeometryTypes.Mesh:
if (geometry != null)
{
geometryGameObject = CreateMeshCollider(geometry.mesh);
}
else
{
geometryGameObject = new GameObject(geometryType.ToString());
geometryGameObject.AddComponent <MeshCollider>();
}
//var collider = geometryGameObject.GetComponent<MeshCollider>();
//collider.convex = true;
break;
}
if (geometryGameObject != null)
{
geometryGameObject.transform.SetParentAndAlign(parent);
if (geometry != null)
{
SetScale(parent, geometry, geometryType);
}
}
}
public static void Create(Transform parent, GeometryTypes geometryType, Link.Geometry geometry = null)
{
GameObject geometryGameObject = null;
switch (geometryType)
{
case GeometryTypes.Box:
geometryGameObject = GameObject.CreatePrimitive(PrimitiveType.Cube);
geometryGameObject.transform.DestroyImmediateIfExists <BoxCollider>();
break;
case GeometryTypes.Cylinder:
geometryGameObject = GameObject.CreatePrimitive(PrimitiveType.Cylinder);
geometryGameObject.transform.DestroyImmediateIfExists <CapsuleCollider>();
break;
case GeometryTypes.Sphere:
geometryGameObject = GameObject.CreatePrimitive(PrimitiveType.Sphere);
geometryGameObject.transform.DestroyImmediateIfExists <SphereCollider>();
break;
case GeometryTypes.Mesh:
if (geometry != null)
{
geometryGameObject = CreateMeshVisual(geometry.mesh);
}
//else, let user add their own mesh gameObject
break;
}
if (geometryGameObject != null)
{
geometryGameObject.transform.SetParentAndAlign(parent);
if (geometry != null)
{
SetScale(parent, geometry, geometryType);
}
}
}
public static bool CheckForUrdfCompatibility(Transform transform, GeometryTypes type)
{
if (transform.childCount == 0)
{
Debug.LogWarning($"No elements found in {transform}! Cannot export to URDF.");
return(false);
}
Transform childTransform = transform.GetChild(0);
if (IsTransformed(childTransform, type))
{
Debug.LogWarning($"Changes to the transform of {childTransform.name} cannot be exported to URDF. Make any translation, rotation, or scale changes to the parent Visual or Collision object instead.", childTransform);
return(false);
}
if (!transform.HasExactlyOneChild())
{
Debug.LogWarning($"Only one Geometry element is allowed for each Visual or Collision element. In {transform.root.name} move each Geometry into its own Visual or Collision.", transform);
return(false);
}
return(true);
}
/// <summary>
/// 3D SZ !!!!
/// Creates a dictionary with key equal to the model state index and the value the spatial information of that state index.
/// </summary>
/// <param name="gType">The geometric type of the exchange itme (2d or 3d)</param>
/// <param name="baseOut">The exchange item base output</param>
/// <param name="elementID">the string id of the exchange item.</param>
/// <returns></returns>
private IDictionary <int, ISpatialDefine> GetModelCoordinates3DSZ(GeometryTypes gType, IBaseOutput baseOut, string elementID)
{
//Run Only once - because it's slow
if (_modelEntities == null)
{
_modelEntities = new Dictionary <int, ISpatialDefine>();
int n;
try
{
WMEngine.GetElementCount(elementID);
n = baseOut.ElementSet().ElementCount;
}
catch
{
Console.WriteLine("\nElement {0} does not found in the model\n", elementID);
throw new Exception("\nProblem in Model Instance - unable to find exchange item\n");
}
//int numBaseGrid = Convert.ToInt32(Math.Floor((double)n / (double)_mshe.WMEngine.NumberOfSZLayers));
for (int i = 0; i < n; i++)
{
XYPolygon modelpolygon = ElementMapper.CreateXYPolygon(baseOut.ElementSet(), i);
int zLayer = Convert.ToInt32(i % base.WMEngine.NumberOfSZLayers);
// Points in Polygon are defined as LL, LR, UR, UL (l/l = lower/left, u = upper, r = right )
// Finds the mid x and mid y point in the polygon (assuming rectangular grid)
IXYLayerPoint min = new XYLayerPoint(modelpolygon.GetX(0), modelpolygon.GetY(0), zLayer);
IXYLayerPoint max = new XYLayerPoint(modelpolygon.GetX(1), modelpolygon.GetY(3), zLayer);
_modelEntities.Add(i, new SpatialDefine(min, max, GeometryTypes.Geometry3DSZ));
}
}
return(_modelEntities);
}
public static bool IsTransformed(Transform transform, GeometryTypes type)
{
return(transform.localPosition != Vector3.zero ||
transform.localScale != Vector3.one ||
(type != GeometryTypes.Mesh && transform.localRotation != Quaternion.identity));
}
public void GetGeometryTypes_All_DefaultEnums(Geometry geometry, Vector3 scale, GeometryTypes type)
{
Assert.AreEqual(type, UrdfGeometry.GetGeometryType(geometry));
}
public void SetScale_Box_IncreaseLocalScale(Geometry geometry, Vector3 scale, GeometryTypes type)
{
var parent = new GameObject("Parent").transform;
UrdfGeometry.SetScale(parent, geometry, UrdfGeometry.GetGeometryType(geometry));
Assert.AreEqual(scale, parent.transform.localScale);
Object.DestroyImmediate(parent.gameObject);
}
/// <summary>
/// Default Constructor
/// </summary>
public SpatialDefine()
{
_pt1 = new XYLayerPoint(-9999, -9999, -9999);
_pt2 = new XYLayerPoint(-9999, -9999, -9999);
_geometry = GeometryTypes.GeometryPoint;
}
/// <summary>
/// Constructor 1D
/// </summary>
public SpatialDefine(IXYLayerPoint pt1)
{
_pt1 = pt1;
_geometry = GeometryTypes.GeometryPoint;
}
