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}; }