///<summary>
/// Compute some angle using rotation matrix
///</summary>
private void angleMatrix(Skeleton skel)
{
//ELBOWYAW
//Left
Matrix3x3 m_l = new Matrix3x3(skel.BoneOrientations[JointType.ElbowLeft].HierarchicalRotation.Matrix);
Vecto3Float v_l = Kinematic.computeEulerFromMatrixXYZ(m_l);
this.jointAngles[NAOConversion.LElbowYaw] = this.jointAngles[NAOConversion.LElbowYaw]
+ NAOConversion.convertAngle(NAOConversion.LElbowYaw, v_l.Z) / this.avg;
//Right
Matrix3x3 m_r = new Matrix3x3(skel.BoneOrientations[JointType.ElbowRight].HierarchicalRotation.Matrix);
Vecto3Float v_r = Kinematic.computeEulerFromMatrixXYZ(m_r);
this.jointAngles[NAOConversion.RElbowYaw] = this.jointAngles[NAOConversion.RElbowYaw]
+ NAOConversion.convertAngle(NAOConversion.RElbowYaw, v_r.Z) / this.avg;
//HEAD
Matrix3x3 m_head = new Matrix3x3(skel.BoneOrientations[JointType.Head].HierarchicalRotation.Matrix);
Vecto3Float v_head = Kinematic.computeEulerFromMatrixXYZ(m_head);
//Yaw
this.jointAngles[NAOConversion.HeadYaw] = this.jointAngles[NAOConversion.HeadYaw]
+ NAOConversion.convertAngle(NAOConversion.HeadYaw, v_head.Z) / this.avg;
//Pitch
this.jointAngles[NAOConversion.HeadPitch] = this.jointAngles[NAOConversion.HeadPitch]
+ NAOConversion.convertAngle(NAOConversion.HeadPitch, v_head.X) / this.avg;
}
///<summary>
/// Compute some angle using rotation matrix
///</summary>
private void angleMatrix(Skeleton skel)
{
//HEAD
Matrix3x3 m_head = new Matrix3x3(skel.BoneOrientations[JointType.Head].HierarchicalRotation.Matrix);
Vecto3Float v_head = Kinematic.computeEulerFromMatrixXYZ(m_head);
//Yaw
this.jointAngles[NAOConversion.HeadYaw] = this.jointAngles[NAOConversion.HeadYaw]
+ NAOConversion.convertAngle(NAOConversion.HeadYaw, v_head.Z) / this.avg;
//Pitch
this.jointAngles[NAOConversion.HeadPitch] = this.jointAngles[NAOConversion.HeadPitch]
+ NAOConversion.convertAngle(NAOConversion.HeadPitch, v_head.X) / this.avg;
}