Skip to content

Commit 1b22571

Browse files
epernodepernod
authored
[src] Add support for SOFA Data RigidCoord and Remove counter check for simple Data types (#213)
* [src] Remove counter check for simple Data types * [src] Add support for SOFA Data RigidCoord
1 parent 0dc590b commit 1b22571

4 files changed

Lines changed: 182 additions & 18 deletions

File tree

‎Core/Plugins/SofaUnityAPI/BaseAPI/SofaBaseComponentAPI.cs‎

Lines changed: 12 additions & 6 deletions
Original file line numberDiff line numberDiff line change
@@ -561,15 +561,21 @@ public void SetVector4Value(string dataName, Vector4 values, bool doubleValue =
561561

562562
/// Generic method to get value of a Data<Rigid3f> field. </summary>
563563
/// <param name="dataName"> Name of the Data field requested. </param>
564-
/// <param name="values"> Buffer to get the values of the Rigid3f. </param>
564+
/// <param name="values"> Buffer to get the values of the Rigid3 float. </param>
565+
/// <param name="doubleValue"> Parameter to inform if data is in double and should be converted to float. </param>
565566
/// <returns></returns>
566-
public int GetRigidfValue(string dataName, float[] values)
567+
public int GetRigidValue(string dataName, float[] values, bool doubleValue = false)
567568
{
568569
if (checkNativePointer())
569570
{
570-
int res = sofaComponentAPI_getRigid3fValue(m_simu, m_name, dataName, values);
571+
int res = -1;
572+
if (doubleValue)
573+
res = sofaComponentAPI_getRigid3Value(m_simu, m_name, dataName, true, values);
574+
else
575+
res = sofaComponentAPI_getRigid3fValue(m_simu, m_name, dataName, values);
576+
571577
if (res != 0)
572-
Debug.LogError("Method getRigidfValue of Data: " + dataName + " of object: " + m_name + " returns error: " + SofaDefines.msg_error[res]);
578+
Debug.LogError("Method GetRigidValue of Data: " + dataName + " of object: " + m_name + " returns error: " + SofaDefines.msg_error[res]);
573579
return res;
574580
}
575581

@@ -1070,10 +1076,10 @@ public int SetVecofVec3Value(string dataName, int size, float[] values, bool dou
10701076

10711077
/// Rigid3 API
10721078
[DllImport("SofaVerseAPI", CharSet = CharSet.Ansi, CallingConvention = CallingConvention.StdCall)]
1073-
public static extern int sofaComponentAPI_getRigid3Value(IntPtr obj, string componentName, string dataName, bool doubleValue, int[] values);
1079+
public static extern int sofaComponentAPI_getRigid3Value(IntPtr obj, string componentName, string dataName, bool doubleValue, float[] values);
10741080

10751081
[DllImport("SofaVerseAPI", CharSet = CharSet.Ansi, CallingConvention = CallingConvention.StdCall)]
1076-
public static extern int sofaComponentAPI_setRigid3Value(IntPtr obj, string componentName, string dataName, bool doubleValue, int[] values);
1082+
public static extern int sofaComponentAPI_setRigid3Value(IntPtr obj, string componentName, string dataName, bool doubleValue, float[] values);
10771083

10781084

10791085

‎Core/Scripts/Core/Data/SofaData.cs‎

Lines changed: 134 additions & 12 deletions
Original file line numberDiff line numberDiff line change
@@ -49,9 +49,7 @@ public bool Value
4949
{
5050
get
5151
{
52-
if (m_isDirty) // nothing to do
53-
GetValueImpl();
54-
52+
GetValueImpl();
5553
return m_value;
5654
}
5755

@@ -136,9 +134,7 @@ public int Value
136134
{
137135
get
138136
{
139-
if (m_isDirty) // nothing to do
140-
GetValueImpl();
141-
137+
GetValueImpl();
142138
return m_value;
143139
}
144140

@@ -229,9 +225,7 @@ public float Value
229225
{
230226
get
231227
{
232-
if (m_isDirty) // nothing to do
233-
GetValueImpl();
234-
228+
GetValueImpl();
235229
return m_value;
236230
}
237231

@@ -311,9 +305,7 @@ public float Value
311305
{
312306
get
313307
{
314-
if (m_isDirty) // nothing to do
315-
GetValueImpl();
316-
308+
GetValueImpl();
317309
return m_value;
318310
}
319311

@@ -879,4 +871,134 @@ protected override bool GetValueImpl()
879871
}
880872
}
881873

874+
875+
/// <summary>
876+
/// Specialization of the class SofaBaseData to handle SOFA Data<Vec4f> and Data<Vec4d>
877+
/// float or double info will be stored @sa m_isDouble
878+
/// </summary>
879+
[System.Serializable]
880+
public class SofaRigidData : SofaBaseData
881+
{
882+
[SerializeField]
883+
protected Vector3 m_position;
884+
885+
[SerializeField]
886+
protected Quaternion m_orientation;
887+
888+
/// Internal info to know if storing float or double
889+
[SerializeField]
890+
protected bool m_isDouble;
891+
892+
private float[] m_buff = new float[7];
893+
894+
////////////////////////////////////////////
895+
////// SofaVec3Data API /////
896+
////////////////////////////////////////////
897+
898+
/// Default constructor taking the value, the component owner and its name. Will set the type internally
899+
public SofaRigidData(SofaBaseComponent owner, string dataName, bool isDouble)
900+
: base(owner, dataName, "Vec3")
901+
{
902+
m_position = Vector3.zero;
903+
m_orientation = Quaternion.identity;
904+
m_isDouble = isDouble;
905+
906+
for (int i = 0; i < 6; i++)
907+
m_buff[i] = 0;
908+
m_buff[6] = 1;
909+
910+
GetValueImpl();
911+
}
912+
913+
/// Getter/Setter of the @sa m_value. Will call @sa SetValueImpl and @sa GetValueImpl internally for Sofa communication
914+
public Vector3 Position
915+
{
916+
get
917+
{
918+
if (m_isDirty) // nothing to do
919+
GetValueImpl();
920+
921+
return m_position;
922+
}
923+
924+
set
925+
{
926+
if (m_position != value)
927+
{
928+
m_position = value;
929+
if (SetValueImpl())
930+
{
931+
m_owner.m_impl.ReinitComponent();
932+
m_isEdited = true;
933+
}
934+
}
935+
}
936+
}
937+
938+
939+
public Vector3 Angles
940+
{
941+
get
942+
{
943+
if (m_isDirty) // nothing to do
944+
GetValueImpl();
945+
946+
Vector3 angles = new Vector3(m_orientation.eulerAngles[0], -m_orientation.eulerAngles[1], -m_orientation.eulerAngles[2]);
947+
return angles;
948+
}
949+
950+
set
951+
{
952+
//if (m_orientation != value)
953+
//{
954+
// m_orientation = value;
955+
// if (SetValueImpl())
956+
// {
957+
// m_owner.m_impl.ReinitComponent();
958+
// m_isEdited = true;
959+
// }
960+
//}
961+
}
962+
}
963+
964+
/// Log Method for Debug info
965+
public void Log()
966+
{
967+
Debug.Log(m_owner.UniqueNameId + "{" + m_dataType + "}: " + m_dataName + " => " + m_position + " | " + m_orientation);
968+
}
969+
970+
971+
////////////////////////////////////////////
972+
////// SofaVec3Data internal API /////
973+
////////////////////////////////////////////
974+
975+
/// Internal Method to set value inside Sofa simulation
976+
protected override bool SetValueImpl()
977+
{
978+
if (m_owner.m_impl == null)
979+
return false;
980+
981+
//m_owner.m_impl.SetVector3Value(m_dataName, m_value, m_isDouble);
982+
return true;
983+
}
984+
985+
/// Internal Method to get value from Sofa simulation
986+
protected override bool GetValueImpl()
987+
{
988+
if (m_owner.m_impl == null)
989+
return false;
990+
991+
int res = m_owner.m_impl.GetRigidValue(m_dataName, m_buff, m_isDouble);
992+
if ( res == 0)
993+
{
994+
// Getting raw values from SOFA, need to inverse left-right hand coordinate system
995+
m_position = new Vector3(-m_buff[0], m_buff[1], m_buff[2]);
996+
m_orientation = new Quaternion(m_buff[3], m_buff[4], m_buff[5], m_buff[6]);
997+
}
998+
999+
m_isDirty = false;
1000+
return true;
1001+
}
1002+
}
1003+
8821004
}

‎Core/Scripts/Core/Data/SofaDataArchiver.cs‎

Lines changed: 30 additions & 0 deletions
Original file line numberDiff line numberDiff line change
@@ -3,6 +3,7 @@
33
using System.Collections.Generic;
44
using UnityEngine;
55
using SofaUnity;
6+
using XCharts.Runtime;
67

78
namespace SofaUnity
89
{
@@ -96,6 +97,14 @@ public void AddData(SofaBaseComponent owner, string dataName, string dataType)
9697
{
9798
AddVec4Data(owner, dataName, true);
9899
}
100+
else if (dataType == "RigidCoord3f")
101+
{
102+
AddRigidData(owner, dataName, false);
103+
}
104+
else if (dataType == "RigidCoord3d")
105+
{
106+
AddRigidData(owner, dataName, true);
107+
}
99108
else if (dataType == "vector<I>" || dataType == "vector < int >" || dataType == "vector<int>")
100109
{
101110
m_dataArray.Add(new SofaDataVectorInt(owner, dataName, dataType));
@@ -124,6 +133,7 @@ public void AddData(SofaBaseComponent owner, string dataName, string dataType)
124133
{
125134
m_dataArray.Add(new SofaDataVectorVec3(owner, dataName, dataType, true));
126135
}
136+
127137
//else if (dataType == "vector < unsigned int >" || dataType == "vector<unsigned int>")
128138

129139
//{
@@ -331,6 +341,18 @@ public SofaVec4Data GetSofaVec4Data(string dataName)
331341
}
332342

333343

344+
public void PrintAllDataAndTypes()
345+
{
346+
foreach (SofaBaseData data in m_dataArray)
347+
{
348+
Debug.Log("Data Name: " + data.DataName + " Type: " + data.DataType);
349+
}
350+
}
351+
352+
353+
////////////////////////////////////////////////
354+
////// Internal Method To Register Data /////
355+
////////////////////////////////////////////////
334356

335357
/// Internal Method to create a String Data and add it to the List. Will create the container if it is the first Data. Called by @sa AdddData
336358
private void AddStringData(SofaBaseComponent owner, string dataName)
@@ -413,6 +435,12 @@ private void AddVec4Data(SofaBaseComponent owner, string dataName, bool isDouble
413435
m_dataArray.Add(new SofaVec4Data(owner, dataName, value, isDouble));
414436
}
415437

438+
private void AddRigidData(SofaBaseComponent owner, string dataName, bool isDouble = false)
439+
{
440+
SofaRigidData data = new SofaRigidData(owner, dataName, isDouble);
441+
m_dataArray.Add(data);
442+
}
443+
416444

417445
/// Internal Method to create a SofaData for unsupported type and add it to the List. Will create the container if it is the first Data. Called by @sa AdddData
418446
private void AddUnssuportedData(SofaBaseComponent owner, string nameID, string type)
@@ -421,5 +449,7 @@ private void AddUnssuportedData(SofaBaseComponent owner, string nameID, string t
421449
}
422450

423451

452+
453+
424454
}
425455
}

‎Core/Scripts/Editor/Components/SofaBaseComponentEditor.cs‎

Lines changed: 6 additions & 0 deletions
Original file line numberDiff line numberDiff line change
@@ -134,6 +134,12 @@ public override void OnInspectorGUI()
134134
SofaVec4Data data = (SofaVec4Data)(bData);
135135
data.Value = EditorGUILayout.Vector4Field(data.DataName, data.Value);
136136
}
137+
else if (dataType == "RigidCoord3f" || dataType == "RigidCoord3d")
138+
{
139+
SofaRigidData data = (SofaRigidData)(bData);
140+
EditorGUILayout.Vector3Field(data.DataName, data.Position);
141+
EditorGUILayout.Vector3Field(data.DataName, data.Angles);
142+
}
137143
else
138144
{
139145
Debug.LogError("Data not handled: " + dataName + " [" + dataType + "]");

0 commit comments

Comments
 (0)