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