public Matrix CalculateRelativeMatrix(AMCSegment seg, ASFJoint asfJoint)
{
if (asfJoint.name == "root")
{
//use root orientation here? prob not
Matrix c = Matrix.Identity;
c =
Matrix.RotationX(MathHelper.ToRadians(seg.Data[3])) *
Matrix.RotationY(MathHelper.ToRadians(seg.Data[4])) * Matrix.RotationZ(MathHelper.ToRadians(seg.Data[5]));
//use root position here? prob not
Matrix b = Matrix.Translation(seg.Data[0], seg.Data[1], seg.Data[2]);
Matrix m = Matrix.Identity;
m = Matrix.Identity;
Matrix l = Matrix.Invert(c) * m * c * b;
return(l);
}
else
{
//WARNING: not using the given order asfJoint.axisOrder
Matrix c = Matrix.RotationX(MathHelper.ToRadians(asfJoint.axis.X))
* Matrix.RotationY(MathHelper.ToRadians(asfJoint.axis.Y))
* Matrix.RotationZ(MathHelper.ToRadians(asfJoint.axis.Z));
Matrix b = Matrix.Translation(asfJoint.parent.direction * asfJoint.parent.length);
Matrix m = Matrix.Identity;
for (int i = 0; i < asfJoint.dof.Length; i++)
{
switch (asfJoint.dof[i])
{
case "rx":
m = m * Matrix.RotationX(MathHelper.ToRadians(seg.Data[i]));
break;
case "ry":
m = m * Matrix.RotationY(MathHelper.ToRadians(seg.Data[i]));
break;
case "rz":
m = m * Matrix.RotationZ(MathHelper.ToRadians(seg.Data[i]));
break;
}
}
Matrix l = Matrix.Invert(c) * m * c * b;
return(l);
}
}
public void ImportAMC(Stream strm)
{
using (StreamReader reader = new StreamReader(strm))
{
var currentSampleIndex = -1;
Samples.Clear();
while (!reader.EndOfStream)
{
var line = reader.ReadLine();
if (line.StartsWith("#") || line.StartsWith(":"))
{
continue;
}
int sampleNumber;
if (int.TryParse(line, out sampleNumber))
{
//This is dangerous
while (Samples.Count <= sampleNumber || Samples[sampleNumber] == null)
{
Samples.Add(new AMCSample());
}
currentSampleIndex = sampleNumber - 1;
continue;
}
string[] parts = line.Split(' ');
var seg = new AMCSegment();
seg.JointName = parts[0];
var cultureInfo = new CultureInfo("en-gb");
seg.Data = parts.Skip(1).Select(str => float.Parse(str, cultureInfo.NumberFormat)).ToArray();
Samples[currentSampleIndex].Segments.Add(seg);
}
}
}
