public void BuildTree()
{
if (_root == null)
{
return;
}
Queue <BodyNode> Q = new Queue <BodyNode>();
Q.Enqueue(_root);
List <BodyNode> tagged = new List <BodyNode>();
tagged.Add(_root);
while (Q.Count > 0)
{
BodyNode node = Q.Dequeue();
List <BodyBone> adjBones = node.getAdjBones();
foreach (BodyBone bone in adjBones)
{
BodyNode other = bone._SRC == node ? bone._DST : bone._SRC;
if (tagged.Contains(other))
{
continue;
}
other._PARENT = node;
node.addChildNode(other);
Q.Enqueue(other);
tagged.Add(other);
}
}
}// BuildTree
public List <BodyNode> getDescendents()
{
List <BodyNode> descendents = new List <BodyNode>();
if (_childrenNodes != null)
{
Queue <BodyNode> Q = new Queue <BodyNode>();
foreach (BodyNode bn in _childrenNodes)
{
Q.Enqueue(bn);
}
while (Q.Count > 0)
{
BodyNode node = Q.Dequeue();
if (!descendents.Contains(node))
{
descendents.Add(node);
}
if (node._childrenNodes != null)
{
foreach (BodyNode bn in node._childrenNodes)
{
Q.Enqueue(bn);
}
}
}
}
return(descendents);
}// getDescendents
public BodyBone(BodyNode s, BodyNode d, string name)
{
_src = s;
_dst = d;
_name = name;
_src.addAdjBone(this);
_dst.addAdjBone(this);
_entity = new Ellipsoid(_len, _wid, _thickness, _nslices);
updateEntity();
}
public BodyBone(BodyNode s, BodyNode d, string name, double w, double th, int n)
{
_src = s;
_dst = d;
_name = name;
_src.addAdjBone(this);
_dst.addAdjBone(this);
_len = (_src._POS - _dst._POS).Length();
_wid = w;
_thickness = th;
_nslices = n;
_entity = new Ellipsoid(_len, _wid, _thickness, _nslices);
updateEntity();
}
public void addChildNode(BodyNode node)
{
_childrenNodes.Add(node);
node._PARENT = this;
}
public void addAdjNode(BodyNode node)
{
_adjNodes.Add(node);
}
} // saveHumanPose
public void loadPose(string filename)
{
if (!File.Exists(filename))
{
return;
}
using (StreamReader sr = new StreamReader(filename))
{
char[] separator = { ' ', '\t' };
string line = sr.ReadLine().Trim();
string[] strs = line.Split(separator);
int nnodes = 0;
try
{
nnodes = Int16.Parse(strs[0]);
}
catch (System.FormatException)
{
return;
}
_bodyNodes = new List <BodyNode>();
for (int i = 0; i < nnodes; ++i)
{
line = sr.ReadLine().Trim();
strs = line.Split(separator);
string name = strs[0];
Vector3d pos = new Vector3d(double.Parse(strs[1]), double.Parse(strs[2]), double.Parse(strs[3]));
BodyNode bn = new BodyNode(name, pos);
_bodyNodes.Add(bn);
if (bn._NAME == "body_hip")
{
bn.setAsRoot();
_root = bn;
}
}
int nbones = 0;
line = sr.ReadLine().Trim();
strs = line.Split(separator);
try
{
nbones = Int16.Parse(strs[0]);
}
catch (System.FormatException)
{
return;
}
_bodyBones = new List <BodyBone>();
for (int i = 0; i < nbones; ++i)
{
line = sr.ReadLine().Trim();
strs = line.Split(separator);
string name = strs[0];
int inode = Int16.Parse(strs[1]);
int jnode = Int16.Parse(strs[2]);
double wid = double.Parse(strs[3]);
double thi = double.Parse(strs[4]);
BodyBone bb = new BodyBone(_bodyNodes[inode], _bodyNodes[jnode], name, wid, thi, 20);
_bodyBones.Add(bb);
}
BuildTree();
} // read
} // loadHuamPose
