static MLStructure getStructure(IEnumerable<UAVObject> data, Type type)
{
int rowCount = data.Count();
MLStructure structure = new MLStructure(type.Name, new int[] { 1, 1 });
var timestamp = data.Select(k => k.timestamp).ToArray();
structure["timestamp"] = new MLUInt32("", timestamp, rowCount);
foreach (var prop in type.GetFields().Where(j => j.FieldType.BaseType == typeof(UAVObjectField)))
{
string name = prop.Name;
double[] fieldata = data.Select( k=> Convert.ToDouble(((UAVObjectField)prop.GetValue(k)).getValue(0))).ToArray();
structure[name] = new MLDouble("", fieldata, rowCount);
}
return structure;
}
private static MLArray CreateTestStruct() {
MLStructure outStruct = new MLStructure("kinectData", new int[] { 1, 1 });
double[] myRealNums = { 1, 2, 3, 4, 5, 6, 7, 8, 9, 10, 11, 12, 13, 14, 15, 16, 17, 18, 19, 20 };
// new MLInt64("IA", myRealNums, myImagNums, length);
// IA: matlab Name (blank for struct)
// realNumbers => Double array of the real parts
// imagNumbers => Double array of the img parts
// length => dimension of the matrix
outStruct["skelData", 0] = new MLDouble("", myRealNums, 2);
outStruct["dateHeader", 0] = new MLChar("", "January 29th... etc...");
MLStructure groundStruct = new MLStructure("", new int[] { 1, 1 });
groundStruct["height", 0] = new MLDouble("", new double[] { 0.68 }, 1); // metres?
groundStruct["gpVector", 0] = new MLDouble("", new double[] {1.4, 1, 1, 1}, 4); //metres?
groundStruct["kinectTilt", 0] = new MLInt16("", new Int16[]{ -15 }, 1); //degrees
outStruct["groundPlaneData", 0] = groundStruct;
return outStruct;
}
private static MLArray CreateStructArray()
{
MLStructure structure = new MLStructure("X", new int[] { 1, 1 });
structure["w", 0] = new MLUInt8("", new byte[] { 1 }, 1);
structure["y", 0] = new MLUInt8("", new byte[] { 2 }, 1);
structure["z", 0] = new MLUInt8("", new byte[] { 3 }, 1);
return structure;
}
/// <summary>
/// Reads <c>miMATRIX</c> from the input stream.
/// </summary>
/// <remarks>
/// If reading was not finished (which is normal for filtered results)
/// returns <c>null</c>.
/// </remarks>
/// <param name="buf">The <c>BinaryReader</c> input stream.</param>
/// <param name="isRoot">When <c>true</c> informs that if this is a top level
/// matrix.</param>
/// <returns><c>MLArray</c> or <c>null</c> if matrix does not match <c>filter</c></returns>
private MLArray ReadMatrix( Stream buf, bool isRoot )
{
// result
MLArray mlArray = null;
ISMatTag tag;
// read flags
int[] flags = ReadFlags( buf );
int attributes = (flags.Length != 0 ) ? flags[0] : 0;
int nzmax = ( flags.Length != 0 ) ? flags[1] : 0;
int type = attributes & 0xff;
// read Array dimension
int[] dims = ReadDimension( buf );
// read Array name
string name = ReadName( buf );
// If this array is filtered out return immediately
if( isRoot && !_filter.Matches(name) )
{
return null;
}
// read data
switch (type)
{
case MLArray.mxSTRUCT_CLASS:
MLStructure mlStruct = new MLStructure(name, dims, type, attributes);
BinaryReader br = new BinaryReader(buf);
// field name length - this subelement always uses the compressed data element format
tag = new ISMatTag(br.BaseStream);
int maxlen = br.ReadInt32();
// Read fields data as Int8
tag = new ISMatTag(br.BaseStream);
// calculate number of fields
int numOfFields = tag.Size / maxlen;
// padding after field names
int padding = (tag.Size % 8) != 0 ? 8 - tag.Size % 8 : 0;
string[] fieldNames = new string[numOfFields];
for (int i = 0; i < numOfFields; i++)
{
byte[] names = new byte[maxlen];
br.Read(names, 0, names.Length);
fieldNames[i] = ZeroEndByteArrayToString(names);
}
br.ReadBytes(padding);
// read fields
for (int index = 0; index < mlStruct.M * mlStruct.N; index++)
{
for (int i = 0; i < numOfFields; i++)
{
// read matrix recursively
tag = new ISMatTag(br.BaseStream);
if (tag.Size > 0)
{
MLArray fieldValue = ReadMatrix(br.BaseStream, false);
mlStruct[fieldNames[i], index] = fieldValue;
}
else
{
mlStruct[fieldNames[i], index] = new MLEmptyArray();
}
}
}
mlArray = mlStruct;
//br.Close();
break;
case MLArray.mxCELL_CLASS:
MLCell cell = new MLCell(name, dims, type, attributes);
for (int i = 0; i < cell.M * cell.N; i++)
{
tag = new ISMatTag(buf);
if (tag.Size > 0)
{
MLArray cellmatrix = ReadMatrix(buf, false);
cell[i] = cellmatrix;
}
else
{
cell[i] = new MLEmptyArray();
}
}
mlArray = cell;
break;
case MLArray.mxDOUBLE_CLASS:
mlArray = new MLDouble(name, dims, type, attributes);
//read real
tag = new ISMatTag(buf);
tag.ReadToByteBuffer(((MLNumericArray<double>)mlArray).RealByteBuffer,
(ByteStorageSupport)mlArray);
// read complex
if (mlArray.IsComplex)
{
tag = new ISMatTag(buf);
tag.ReadToByteBuffer(((MLNumericArray<double>)mlArray).ImaginaryByteBuffer,
(ByteStorageSupport)mlArray);
}
break;
case MLArray.mxUINT8_CLASS:
mlArray = new MLUInt8(name, dims, type, attributes);
//read real
tag = new ISMatTag(buf);
tag.ReadToByteBuffer(((MLNumericArray<byte>)mlArray).RealByteBuffer,
(ByteStorageSupport)mlArray);
// read complex
if (mlArray.IsComplex)
{
tag = new ISMatTag(buf);
tag.ReadToByteBuffer(((MLNumericArray<byte>)mlArray).ImaginaryByteBuffer,
(ByteStorageSupport)mlArray);
}
break;
case MLArray.mxINT8_CLASS:
mlArray = new MLInt8(name, dims, type, attributes);
//read real
tag = new ISMatTag(buf);
tag.ReadToByteBuffer(((MLNumericArray<byte>)mlArray).RealByteBuffer,
(ByteStorageSupport)mlArray);
// read complex
if (mlArray.IsComplex)
{
tag = new ISMatTag(buf);
tag.ReadToByteBuffer(((MLNumericArray<byte>)mlArray).ImaginaryByteBuffer,
(ByteStorageSupport)mlArray);
}
break;
case MLArray.mxUINT64_CLASS:
mlArray = new MLUInt64(name, dims, type, attributes);
//read real
tag = new ISMatTag(buf);
tag.ReadToByteBuffer(((MLNumericArray<byte>)mlArray).RealByteBuffer,
(ByteStorageSupport)mlArray);
// read complex
if (mlArray.IsComplex)
{
tag = new ISMatTag(buf);
tag.ReadToByteBuffer(((MLNumericArray<byte>)mlArray).ImaginaryByteBuffer,
(ByteStorageSupport)mlArray);
}
break;
case MLArray.mxINT64_CLASS:
mlArray = new MLInt64(name, dims, type, attributes);
//read real
tag = new ISMatTag(buf);
tag.ReadToByteBuffer(((MLNumericArray<byte>)mlArray).RealByteBuffer,
(ByteStorageSupport)mlArray);
// read complex
if (mlArray.IsComplex)
{
tag = new ISMatTag(buf);
tag.ReadToByteBuffer(((MLNumericArray<byte>)mlArray).ImaginaryByteBuffer,
(ByteStorageSupport)mlArray);
}
break;
case MLArray.mxCHAR_CLASS:
MLChar mlchar = new MLChar(name, dims, type, attributes);
//read real
tag = new ISMatTag(buf);
char[] ac = tag.ReadToCharArray();
for (int i = 0; i < ac.Length; i++)
{
mlchar.SetChar(ac[i], i);
}
mlArray = mlchar;
break;
case MLArray.mxSPARSE_CLASS:
MLSparse sparse = new MLSparse(name, dims, attributes, nzmax);
// read ir (row indices)
tag = new ISMatTag(buf);
int[] ir = tag.ReadToIntArray();
// read jc (column indices)
tag = new ISMatTag(buf);
int[] jc = tag.ReadToIntArray();
// read pr (real part)
tag = new ISMatTag(buf);
double[] ad1 = tag.ReadToDoubleArray();
int n = 0;
for (int i = 0; i < ir.Length; i++)
{
if (i < sparse.N)
{
n = jc[i];
}
sparse.SetReal(ad1[i], ir[i], n);
}
//read pi (imaginary part)
if (sparse.IsComplex)
{
tag = new ISMatTag(buf);
double[] ad2 = tag.ReadToDoubleArray();
int n1 = 0;
for (int i = 0; i < ir.Length; i++)
{
if (i < sparse.N)
{
n1 = jc[i];
}
sparse.SetImaginary(ad2[i], ir[i], n1);
}
}
mlArray = sparse;
break;
default:
throw new MatlabIOException("Incorrect Matlab array class: " + MLArray.TypeToString(type));
}
return mlArray;
}
public static void SaveMatFile()
{
// create a MATLAB char and double variable and add it to the structure
//MLChar scoreName = ;
//MLDouble scoreValue = new MLDouble("", new double[] { 12345 }, 1);
int maxIter = 0;
foreach(ActionData ad in ActionData.actionList)
{
if(ad.iterationNum > maxIter)
{
maxIter = ad.iterationNum;
}
}
string dateLbl = "";
dateLbl += System.DateTime.Now.Year.ToString();
dateLbl += System.DateTime.Now.Month.ToString();
dateLbl += System.DateTime.Now.Day.ToString();
dateLbl += System.DateTime.Now.ToFileTimeUtc().ToString();
int chunkSize = 50;
int effChunkSize = 50;
int chunk = 0;
int actionIndex = 0;
List<string> fieldList = new List<string> ();
fieldList.Add ("obstacle");
fieldList.Add ("goal");
fieldList.Add ("item");
fieldList.Add ("robot");
fieldList.Add ("selectedItem");
while((chunk)*chunkSize < ActionData.actionList.Count)
{
int i = 0;
int ii = 0;
actionIndex = chunk*chunkSize;
if(chunkSize + actionIndex >= ActionData.actionList.Count)
effChunkSize = ActionData.actionList.Count-actionIndex;
else
effChunkSize = chunkSize;
MLStructure structure = new MLStructure("simDat", new int[] { effChunkSize, 1 });
List<MLArray> mlList = new List<MLArray>();
MLDouble arr;
while(actionIndex < ActionData.actionList.Count)
{
ActionData ad = actionList[actionIndex] as ActionData;
structure["speed", i] = new MLDouble("", new double[] {maxIter}, 1);
structure["beforePos", i] = new MLDouble("", new double[] {ad.beforePos.x,ad.beforePos.y,ad.beforePos.z }, 1);
structure["beforeOrient", i] = new MLDouble("", new double[] {ad.beforeOrient.eulerAngles.x,ad.beforeOrient.eulerAngles.y,ad.beforeOrient.eulerAngles.z}, 1);
//Debug.Log(dateLbl+"_"+chunk+"["+i+"]" +"("+ad.beforeOrient.x+","+ad.beforeOrient.y+","+ad.beforeOrient.z+")");
structure["afterPos", i] = new MLDouble("", new double[] {ad.afterPos.x,ad.afterPos.y,ad.afterPos.z}, 1);
structure["afterOrient", i] = new MLDouble("", new double[] {ad.afterOrient.eulerAngles.x,ad.afterOrient.eulerAngles.y,ad.afterOrient.eulerAngles.z}, 1);
structure["robotType", i] = new MLChar("", ad.robotType);
structure["itemType", i] = new MLChar("", ad.itemType);
ii=0;
foreach(string fieldName in fieldList)
{
int id = ad.positionNames.IndexOf(fieldName);
Vector3 vecValue;
if(id >= 0)
vecValue = ad.positionVecs[id];
else
vecValue = new Vector3(100,100,100); // We pass an invalid value
structure[fieldName, i] = new MLDouble("", new double[] {vecValue.x,vecValue.y,vecValue.z}, 1);
ii++;
}
i++;
actionIndex++;
}
mlList.Add(structure);
MatFileWriter mfw = new MatFileWriter("playDat"+dateLbl+"_"+chunk+".mat", mlList, false);
mfw.GetType();
Debug.Log ("Saved (" + chunk + ")playDat" + dateLbl + ".mat");
chunk ++;
ActionData.actionList = new List<ActionData>();
}
}
internal void saveAsMAT()
{
SaveFileDialog saveFileDialog = new SaveFileDialog();
saveFileDialog.Filter = "MAT files (*.mat)|*.mat|All files (*.*)|*.*";
saveFileDialog.DefaultExt = "mat";
saveFileDialog.InitialDirectory = Properties.Settings.Default.saveImpedanceDirectory;
if (saveFileDialog.ShowDialog() == DialogResult.OK)
{
string filename = saveFileDialog.FileName;
string tmpinfo = new FileInfo(filename).DirectoryName;
Properties.Settings.Default.saveImpedanceDirectory = tmpinfo;
List<MLArray> mlList = new List<MLArray>();
MLStructure structure = new MLStructure("imp", new int[] { 1, 1 });
structure["f", 0] = new MLDouble("", freqs, freqs.Length);
//Only add non-null (sampled) channels
int numNonNull = 0;
List<int> goodChannels = new List<int>();
for (int i = 0; i < impedance.Length; ++i)
if (impedance[i] != null)
{
++numNonNull;
goodChannels.Add(i);
}
double[][] nonNullImpedance = new double[numNonNull][];
for (int i = 0; i < numNonNull; ++i) nonNullImpedance[i] = impedance[goodChannels[i]];
structure["z", 0] = new MLDouble("", nonNullImpedance);
mlList.Add(structure);
try
{
MatFileWriter mfw = new MatFileWriter(filename, mlList, true);
}
catch (Exception err)
{
MessageBox.Show("There was an error when creating the MAT-file: \n" + err.ToString(),
"MAT-File Creation Error!", MessageBoxButtons.OK, MessageBoxIcon.Exclamation);
}
}
}
//public void CreateCSVFile(DataTable dt, string strFilePath)
//{
// #region Export Grid to CSV
// // Create the CSV file to which grid data will be exported.
// StreamWriter sw = new StreamWriter(strFilePath, false);
// // First we will write the headers.
// //DataTable dt = m_dsProducts.Tables[0];
// int iColCount = dt.Columns.Count;
// for (int i = 0; i < iColCount; i++)
// {
// sw.Write(dt.Columns[i]);
// if (i < iColCount - 1)
// {
// sw.Write(",");
// }
// }
// sw.Write(sw.NewLine);
// // Now write all the rows.
// foreach (DataRow dr in dt.Rows)
// {
// for (int i = 0; i < iColCount; i++)
// {
// if (!Convert.IsDBNull(dr[i]))
// {
// sw.Write(dr[i].ToString());
// }
// if (i < iColCount - 1)
// {
// sw.Write(",");
// }
// }
// sw.Write(sw.NewLine);
// }
// sw.Close();
// #endregion
//}
private MLArray createStruct()
{
MLStructure outStruct = new MLStructure("kinectData", new int[] { 1, 1 });
//string[] headers = {"HipCenterX", "HipCenterY", "HipCenterZ", "SpineX", "SpineY", "SpineZ", "ShoulderCenterX",
// "ShoulderCenterY", "ShoulderCenterZ", "HeadX", "HeadY", "HeadZ", "ShoulderLeftX",
// "ShoulderLeftY", "ShoulderLeftZ", "ElbowLeftX", "ElbowLeftY", "ElbowLeftZ", "WristLeftX",
// "WristLeftY", "WristLeftZ", "HandLeftX", "HandLeftY", "HandLeftZ", "ShoulderRightX",
// "ShoulderRightY", "ShoulderRightZ", "ElbowRightX", "ElbowRightY", "ElbowRightZ",
// "WristRightX", "WristRightY", "WristRightZ", "HandRightX", "HandRightY", "HandRightZ",
// "HipLeftX", "HipLeftY", "HipLeftZ", "KneeLeftX", "KneeLeftY", "KneeLeftZ", "AnkleLeftX",
// "AnkleLeftY", "AnkleLeftZ", "FootLeftX", "FootLeftY", "FootLeftZ", "HipRightX", "HipRightY",
// "HipRightZ", "KneeRightX", "KneeRightY", "KneeRightZ", "AnkleRightX", "AnkleRightY",
// "AnkleRightZ", "FootRightX", "FootRightY", "FootRightZ"};
double criticalC1D = Convert.ToDouble(criticalC1);
double criticalC2D = Convert.ToDouble(criticalC2);
double criticalC3D = Convert.ToDouble(criticalC3);
double criticalC11D = Convert.ToDouble(criticalC11);
double criticalC21D = Convert.ToDouble(criticalC21);
double criticalC31D = Convert.ToDouble(criticalC31);
double criticalC12D = Convert.ToDouble(criticalC12);
double criticalC22D = Convert.ToDouble(criticalC22);
double criticalC32D = Convert.ToDouble(criticalC32);
double criticalC13D = Convert.ToDouble(criticalC13);
double criticalC23D = Convert.ToDouble(criticalC23);
double criticalC33D = Convert.ToDouble(criticalC33);
double criticalC14D = Convert.ToDouble(criticalC14);
double criticalC24D = Convert.ToDouble(criticalC24);
double criticalC34D = Convert.ToDouble(criticalC34);
double[] ccArray = { criticalC1D, criticalC2D, criticalC3D, criticalC11D, criticalC21D, criticalC31D, criticalC12D, criticalC22D, criticalC32D,
criticalC13D, criticalC23D, criticalC33D, criticalC14D, criticalC24D, criticalC34D};
Double[] skelDataA = (Double[])skelData.ToArray(typeof(double));
Double[] gpVectorA = (Double[])gpVector.ToArray(typeof(double));
Double[] kinectTiltA = (Double[])kinectTilt.ToArray(typeof(double));
outStruct["skelData", 0] = new MLDouble("", skelDataA, 60);
outStruct["dateHeader", 0] = new MLChar("", timeStamp);
outStruct["criticalComps", 0] = new MLDouble("", ccArray, 3);
outStruct["exercise", 0] = new MLChar("", exercise);
MLStructure groundStruct = new MLStructure("", new int[] { 1, 1 });
groundStruct["height", 0] = new MLDouble("", new double[] { 0.68 }, 1); // metres?
groundStruct["gpVector", 0] = new MLDouble("", gpVectorA, 4); //metres?
groundStruct["kinectTilt", 0] = new MLDouble("", kinectTiltA, 1); //degrees
outStruct["groundPlaneData", 0] = groundStruct;
return outStruct;
}
