private static BVHNode ReadNode(StreamReader reader, string idLine, int depth)
{
BVHNode node = new BVHNode();
var line = idLine.ToLower().Trim();
string[] tokens = line.Split(new[] { ' ', '\t' }, StringSplitOptions.RemoveEmptyEntries);
string nodeType = tokens[0];
string nodeName = tokens[1];
BVHNodeTypes type;
if (nodeType == "end" && nodeName == "site")
{
type = BVHNodeTypes.EndSite;
}
else
{
if (!Enum.TryParse(nodeType, true, out type))
{
throw new FileFormatException($"Invalid BVH Node Type: {nodeType}");
}
}
node.Type = type;
node.Name = nodeName;
reader.ReadLine();
node.Offset = ReadOffset(reader);
if (node.Type != BVHNodeTypes.EndSite)
{
node.Channels = ReadChannels(reader);
}
while (true)
{
line = reader.ReadLine().ToLower().Trim();
if (line == "}")
{
return(node);
}
else
{
node.Children.Add(ReadNode(reader, line, depth + 1));
}
}
}
private void LoadBVHFile(FileInfo file)
{
BVHMotionData bvhMotionData;
BVHNode bvhRoot = BVHNode.ReadBVH(file, out bvhMotionData);
MotionData motionData = new MotionData();
motionData.FPS = 1.0 / bvhMotionData.FrameTime;
Bone rootBone = BVHNode.ToBones(bvhRoot, null, bvhMotionData, motionData);
Kinematic = new KinematicVM(new KinematicStructure(rootBone));
Animator = new KinematicAnimatorVM(Kinematic, motionData);
}
public static Bone ToBones(BVHNode bvhNode, Bone parent, BVHMotionData bvhMotionData, MotionData resultMotionData)
{
Bone result = new Bone(parent, name: bvhNode.Name, offset: bvhNode.Offset);
if (bvhNode.Type != BVHNodeTypes.EndSite)
{
resultMotionData.Data.Add(result, bvhMotionData.Data[bvhNode].ToList());
}
foreach (BVHNode item in bvhNode.Children)
{
result.Children.Add(ToBones(item, result, bvhMotionData, resultMotionData));
}
return(result);
}
public static BVHMotionData ReadMotionData(StreamReader reader, BVHNode root)
{
var line = reader.ReadLine().ToLower().Trim();
if (line != "motion")
{
throw new FileFormatException("Expected MOTION keyword");
}
line = reader.ReadLine().ToLower().Trim();
var tokens = line.Split(new[] { ' ', '\t' }, StringSplitOptions.RemoveEmptyEntries);
if (!int.TryParse(tokens[1], out int numFrames))
{
throw new FileFormatException("Could not read number of frames");
}
line = reader.ReadLine().ToLower().Trim();
tokens = line.Split(new[] { ' ', '\t' }, StringSplitOptions.RemoveEmptyEntries);
if (!double.TryParse(tokens[2], NumberStyles.Number, CultureInfo.InvariantCulture, out double frameTime))
{
throw new FileFormatException("Could not read frame time");
}
Dictionary <BVHNode, List <Quaternion> > motionData = new Dictionary <BVHNode, List <Quaternion> >();
while (!reader.EndOfStream)
{
line = reader.ReadLine().ToLower().Trim();
if (String.IsNullOrWhiteSpace(line))
{
continue;
}
double[] frameData = line.Split(new[] { ' ', '\t' }, StringSplitOptions.RemoveEmptyEntries).Select(t => double.Parse(t, NumberStyles.Float, CultureInfo.InvariantCulture)).ToArray();
int offset = 0;
ReadFrameData(root, motionData, frameData, ref offset);
}
return(new BVHMotionData(frameTime, motionData));
}
private static void ReadFrameData(BVHNode node, Dictionary <BVHNode, List <Quaternion> > motionData, double[] values, ref int offset)
{
if (node.Type == BVHNodeTypes.EndSite)
{
return;
}
double[] nodevalues = new double[node.Channels.Length];
Array.Copy(values, offset, nodevalues, 0, node.Channels.Length);
offset += node.Channels.Length;
if (!motionData.ContainsKey(node))
{
motionData.Add(node, new List <Quaternion>());
}
int ignoredOffset = 0;
if (node.Channels[0] == BVHChannels.Xposition)
{
ignoredOffset += 3;
}
var q1 = new Quaternion(GetAxisFromChannelType(node.Channels[ignoredOffset]), nodevalues[ignoredOffset]);
var q2 = new Quaternion(GetAxisFromChannelType(node.Channels[ignoredOffset + 1]), nodevalues[ignoredOffset + 1]);
var q3 = new Quaternion(GetAxisFromChannelType(node.Channels[ignoredOffset + 2]), nodevalues[ignoredOffset + 2]);
Quaternion quat = q1 * q2 * q3;
motionData[node].Add(quat);
foreach (var item in node.Children)
{
ReadFrameData(item, motionData, values, ref offset);
}
}