public static float grade_pose(Pose A, Pose B, bool useGrace = true) //weight is taken from Pose B, B is traget
{
float powwow = 2.5f;
float weightsum = 0;
float gradesum = 0;
string output = "";
foreach (PoseElement e in A.mElements)
{
//Debug.Log(e.joint);
PoseElement bPose = B.find_element(e.joint);
float target = bPose.angle;
float actual = e.angle;
float diff = target - actual;
while (diff > 180)
{
diff -= 360;
}
while (diff < -180)
{
diff += 360;
}
diff = Mathf.Abs(diff);
//grading grace
if (useGrace)
{
diff = Mathf.Max(0, diff - GameConstants.gradingGraceDegrees);
}
gradesum += Mathf.Pow(diff, powwow) * bPose.weight;
weightsum += bPose.weight;
output += e.joint + " target: " + target + " actual: " + actual + " diff: " + diff + "\n";
}
output += " grade: " + Mathf.Pow(gradesum, 1f / powwow) / weightsum;
return(Mathf.Pow(gradesum, 1f / powwow) / weightsum);
}
public static Dictionary <ZgJointId, float> advanced_grade_pose(Pose A, Pose B)
{
Dictionary <ZgJointId, float> r = new Dictionary <ZgJointId, float>();
float weightSum = 0;
foreach (PoseElement e in A.mElements)
{
weightSum += B.find_element(e.joint).weight;
}
foreach (PoseElement e in A.mElements)
{
PoseElement bPose = B.find_element(e.joint);
float target = bPose.angle;
float actual = e.angle;
float diff = target - actual;
while (diff > 180)
{
diff -= 360;
}
while (diff < -180)
{
diff += 360;
}
r[e.joint] = diff;
}
return(r);
}
public static Pose read_pose(string aPose)
{
Pose p = new Pose();
//System.IO.MemoryStream stream = new System.IO.MemoryStream(aText.bytes);
var tr = new System.IO.StringReader(aPose);
var xr = System.Xml.XmlReader.Create(tr);
var xd = new System.Xml.XmlDocument();
xd.Load(xr);
var elts = xd.ChildNodes [1].FirstChild;
for (int i = 0; i < elts.ChildNodes.Count; i++)
{
var active = elts.ChildNodes[i];
var activeElement = new PoseElement();
for (int j = 0; j < active.ChildNodes.Count; j++)
{
if (active.ChildNodes[j].Name == "joint")
{
activeElement.joint = parse_ZigJointId_string(active.ChildNodes[j].InnerXml);
}
else if (active.ChildNodes[j].Name == "angle")
{
activeElement.angle = float.Parse(active.ChildNodes[j].InnerXml);
}
}
p.mElements.Add(activeElement);
}
//System.Xml.Serialization.XmlSerializer xs = new System.Xml.Serialization.XmlSerializer(typeof(Pose));
//p = (Pose)xs.Deserialize(stream);
//p = (Pose)xs.Deserialize(tr);
return(p);
}
public void keyboard_update()
{
if (mFlat != null)
{
//US
/*var keyBindings = new[]{
* new { k = ZigJointId.Neck, u = KeyCode.T, d = KeyCode.Y },
* new { k = ZigJointId.Torso, u = KeyCode.G, d = KeyCode.H },
* new { k = ZigJointId.Waist, u = KeyCode.B, d = KeyCode.N },
* new { k = ZigJointId.LeftShoulder, u = KeyCode.E, d = KeyCode.R },
* new { k = ZigJointId.RightShoulder, u = KeyCode.U, d = KeyCode.I },
* new { k = ZigJointId.LeftElbow, u = KeyCode.Q, d = KeyCode.W },
* new { k = ZigJointId.RightElbow, u = KeyCode.O, d = KeyCode.P },
* new { k = ZigJointId.LeftHip, u = KeyCode.C, d = KeyCode.V },
* new { k = ZigJointId.RightHip, u = KeyCode.M, d = KeyCode.Comma },
* new { k = ZigJointId.LeftKnee, u = KeyCode.Z, d = KeyCode.X },
* new { k = ZigJointId.RightKnee, u = KeyCode.Period, d = KeyCode.Slash }
* };*/
//GERMAN
var keyBindings = new[] {
new { k = ZgJointId.Neck, u = KeyCode.T, d = KeyCode.Z },
new { k = ZgJointId.Torso, u = KeyCode.G, d = KeyCode.H },
new { k = ZgJointId.Waist, u = KeyCode.B, d = KeyCode.N },
new { k = ZgJointId.LeftShoulder, u = KeyCode.E, d = KeyCode.R },
new { k = ZgJointId.RightShoulder, u = KeyCode.U, d = KeyCode.I },
new { k = ZgJointId.LeftElbow, u = KeyCode.Q, d = KeyCode.W },
new { k = ZgJointId.RightElbow, u = KeyCode.O, d = KeyCode.P },
new { k = ZgJointId.LeftHip, u = KeyCode.C, d = KeyCode.V },
new { k = ZgJointId.RightHip, u = KeyCode.M, d = KeyCode.Comma },
new { k = ZgJointId.LeftKnee, u = KeyCode.Y, d = KeyCode.X },
new { k = ZgJointId.RightKnee, u = KeyCode.Period, d = KeyCode.Minus }
};
Pose p = new Pose();
foreach (KeyValuePair <ZgJointId, GameObject> e in mFlat.mParts)
{
PoseElement pe = new PoseElement();
pe.joint = e.Key;
pe.angle = e.Value.transform.rotation.eulerAngles.z;
p.mElements.Add(pe);
}
foreach (var e in keyBindings)
{
if (Input.GetKey(e.u))
{
p.find_element(e.k).angle += 1;
}
else if (Input.GetKey(e.d))
{
p.find_element(e.k).angle -= 1;
}
}
mFlat.set_target_pose(p, true);
}
}
public Pose physics_pose()
{
Pose r = new Pose();
foreach (var e in mBodies)
{
PoseElement pe = new PoseElement();
pe.joint = e.Key;
pe.angle = e.Value.offset + e.Value.body.Rotation / Mathf.PI * 180;
r.mElements.Add(pe);
}
return(r);
}
public static Pose interpolate(Pose A, Pose B, float lambda)
{
Pose r = new Pose();
foreach (PoseElement e in A.mElements)
{
PoseElement pe = new PoseElement();
pe.joint = e.joint;
pe.important = e.important;
pe.weight = e.weight;
pe.angle = VectorMathUtilities.MathHelper.interpolate_degrees(e.angle, B.find_element(e.joint).angle, lambda);
r.mElements.Add(pe);
}
return(r);
}
public static float grade_joint(Pose A, Pose B, ZgJointId aJoint)
{
PoseElement e = A.find_element(aJoint);
PoseElement bPose = B.find_element(aJoint);
float target = bPose.angle;
float actual = e.angle;
float diff = target - actual;
while (diff > 180)
{
diff -= 360;
}
while (diff < -180)
{
diff += 360;
}
return(diff);
}
public Pose get_pose()
{
Pose p = new Pose();
foreach (KeyValuePair <ZgJointId, GameObject> e in mParts)
{
if (e.Key != ZgJointId.LeftHand &&
e.Key != ZgJointId.RightHand &&
e.Key != ZgJointId.LeftAnkle &&
e.Key != ZgJointId.RightAnkle &&
e.Key != ZgJointId.Head)
{
PoseElement pe = new PoseElement();
pe.joint = e.Key;
pe.angle = e.Value.transform.rotation.eulerAngles.z;
p.mElements.Add(pe);
}
}
return(p);
}
public static Pose snap_pose(ManagerManager manager)
{
Pose p = new Pose();
foreach (KeyValuePair <ZgJointId, ZgJointId> e in sPairs)
{
PoseElement pe = new PoseElement();
//ZigInputJoint A = manager.mZigManager.Joints[e.Key];
//ZigInputJoint B = manager.mZigManager.Joints[e.Value];
//pe.angle = manager.mProjectionManager.get_relative(A, B);
pe.weight = 1;
pe.angle = manager.mProjectionManager.get_smoothed_relative(e.Key, e.Value);
pe.joint = e.Key;
p.mElements.Add(pe);
}
PoseElement waist = new PoseElement();
waist.angle = manager.mProjectionManager.mWaist.current;
//waist.angle = manager.mProjectionManager.get_waist(manager.mZigManager.Joints[ZigJointId.Waist], manager.mZigManager.Joints[ZigJointId.LeftHip], manager.mZigManager.Joints[ZigJointId.RightHip]);
waist.joint = ZgJointId.Waist;
p.mElements.Add(waist);
return(p);
}
