/// <summary>
/// reads a bvh node from a given bvh reader
/// </summary>
/// <param name="reader">stream reader standing on the opening parantheses of a node definition</param>
/// <param name="idLine">line containing the name of the node</param>
/// <param name="depth">recursion depth of the current node</param>
private static BVHNode ReadNode(StreamReader reader, string idLine, int depth)
{
BVHNode node = new BVHNode();
// read node type and name
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 <BVHNodeTypes>(nodeType, true, out type))
{
throw new FileFormatException($"Invalid Bvh Node Type: {nodeType}");
}
}
node.Type = type;
node.Name = nodeName;
// read starting curly brace {
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));
}
}
}
/// <summary>
/// recursively write a BVH node an all its children to a BVH motion data file
/// </summary>
private static void WriteBvhNode(BVHNode node, StreamWriter writer, int level)
{
// name and type
for (int i = 0; i < level - 1; ++i)
{
writer.Write("\t");
}
writer.WriteLine($"{GetTypeString(node.Type)} {node.Name}");
// open curly bracket
for (int i = 0; i < level - 1; ++i)
{
writer.Write("\t");
}
writer.WriteLine("{");
// node offset
for (int i = 0; i < level; ++i)
{
writer.Write("\t");
}
writer.WriteLine($"OFFSET {node.Offset.X} {node.Offset.Y} {node.Offset.Z}");
if (node.Type != BVHNodeTypes.EndSite)
{
// defined channels
for (int i = 0; i < level; ++i)
{
writer.Write("\t");
}
writer.Write($"CHANNELS {node.Channels.Length} ");
foreach (var chan in node.Channels)
{
writer.Write(chan.ToString() + " ");
}
writer.Write(Environment.NewLine);
// child nodes
foreach (var child in node.Children)
{
WriteBvhNode(child, writer, level + 1);
}
}
// closing curly bracket
for (int i = 0; i < level - 1; ++i)
{
writer.Write("\t");
}
writer.WriteLine("}");
}
/// <summary>
/// read motion data from a bvh file, starting with the MOTION keyword
/// </summary>
public static BVHMotionData ReadMotionData(StreamReader reader, BVHNode root)
{
var line = reader.ReadLine().ToLower().Trim();
if (line != "motion")
{
throw new FileFormatException("Expected MOTION keyword");
}
// read number of frames
line = reader.ReadLine().ToLower().Trim();
var tokens = line.Split(new[] { ' ', '\t' }, StringSplitOptions.RemoveEmptyEntries);
int numFrames;
if (!int.TryParse(tokens[1], out numFrames))
{
throw new FileFormatException("Could not read number of frames");
}
//read frame time
line = reader.ReadLine().ToLower().Trim();
tokens = line.Split(new[] { ' ', '\t' }, StringSplitOptions.RemoveEmptyEntries);
double frameTime;
if (!double.TryParse(tokens[2], out frameTime))
{
throw new FileFormatException("Could not read frame time");
}
// read all frame data
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)).ToArray();
// interpret frame-by-frame
int offset = 0;
ReadFrameData(root, motionData, frameData, ref offset);
}
return(new BVHMotionData(frameTime, motionData));
}
/// <summary>
/// converts BVH hierarchical structure to the applications own kinematic model definition
/// </summary>
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);
}
/// <summary>
/// interprets motion data for a single frame
/// </summary>
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>());
}
// TODO: add support for position in Bones/check how many channels really have to be skipped
int ignoredOffset = 0;
if (node.Channels[0] == BVHChannels.Xposition)
{
ignoredOffset += 3;
}
// convert rotation to quaternion
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);
}
}
/// <summary>
/// converts a kinematic model to BVH structure for export.
/// </summary>
private static BVHNode ToBVHNode(Bone bone, Dictionary <Bone, List <Quaternion> > sourceMotionData, BVHNode parent, Dictionary <BVHNode, List <Quaternion> > motionData)
{
var result = new BVHNode();
result.Name = bone.Name;
result.Offset = bone.Offset;
// determine type
if (parent == null)
{
result.Type = BVHNodeTypes.Root;
result.Channels = new[] { BVHChannels.Zrotation, BVHChannels.Yrotation, BVHChannels.Xrotation };
}
else if (bone.Children.Count == 0)
{
result.Type = BVHNodeTypes.EndSite;
}
else
{
result.Type = BVHNodeTypes.Joint;
result.Channels = new[] { BVHChannels.Zrotation, BVHChannels.Yrotation, BVHChannels.Xrotation };
}
// populate motion data
if (result.Type != BVHNodeTypes.EndSite)
{
motionData.Add(result, sourceMotionData[bone]);
}
// add child nodes
foreach (var child in bone.Children)
{
result.Children.Add(ToBVHNode(child, sourceMotionData, result, motionData));
}
return(result);
}
/// <summary>
/// Writes a BVH motion data file
/// </summary>
public static void WriteBvh(string file, BVHNode root, BVHMotionData motionData)
{
using (var writer = new StreamWriter(file))
{
writer.WriteLine("HIERARCHY");
WriteBvhNode(root, writer, 0);
writer.WriteLine("MOTION");
int numFrames = motionData.Data.First().Value.Count;
writer.WriteLine($"Frames: {numFrames}");
writer.WriteLine($"Frame Time: {motionData.FrameTime}");
for (int i = 0; i < numFrames; i++)
{
foreach (var node in motionData.Data.Keys)
{
double yaw, pitch, roll;
motionData.Data[node][i].ToYawPitchRoll(out yaw, out pitch, out roll);
writer.Write($"{yaw * 180 / Math.PI} {pitch * 180 / Math.PI} {roll * 180 / Math.PI} ");
}
writer.WriteLine();
}
}
}