public double[] toArray(bool useJointvals)
{
// TODO: convert to joint vals if needed
if (useJointvals)
{
AngleConverter ac = new AngleConverter(this);
return Util.toDoubleArray(ac.getAngles());
}
else
return new double[] {
joints[JointID.HipCenter].x,
joints[JointID.HipCenter].y,
joints[JointID.HipCenter].z};
}