///<summary> /// Receive a rotation matrix of type Matrix4 /// Return a Vector3Float containing the rotation around the 3 axis ///</summary> public static Vecto3Float computeEulerFromMatrixXYZ(Matrix3x3 m) { Vecto3Float sol = new Vecto3Float(); if (m.matrix[0, 2] < 1f) { if (m.matrix[0, 2] > -1f) { sol.X = (float)Math.Atan2(-m.matrix[1, 2], m.matrix[2, 2]); sol.Y = (float)Math.Asin(-(m.matrix[0, 2])); sol.Z = (float)Math.Atan2(-m.matrix[0, 1], m.matrix[0, 0]); } else { sol.X = -(float)Math.Atan2(m.matrix[1, 0], m.matrix[1, 1]); sol.Y = -(float)(Math.PI) / 2; sol.Z = 0f; } } else { sol.X = (float)Math.Atan2(m.matrix[1, 0], m.matrix[1, 1]); sol.Y = (float)(Math.PI) / 2; sol.Z = 0f; } return sol; }
///<summary> /// Receive three skeleton point (3D position of a joint). /// Compute two vector, return the angle between them. /// If something is wrong return 0 ///</summary> public static float getAngle(SkeletonPoint a, SkeletonPoint b, SkeletonPoint c, bool sign) { Vecto3Float[] vector = new Vecto3Float[2]; //vector[0] = new Vecto3Float(b.X - c.X, b.Y - c.Y, b.Z - c.Z); vector[0] = new Vecto3Float(c.X - b.X, c.Y - b.Y, c.Z - b.Z); vector[1] = new Vecto3Float(a.X - b.X, a.Y - b.Y, a.Z - b.Z); float v0_magnitude = vector[0].magnitude(); float v1_magnitude = vector[1].magnitude(); if (v0_magnitude != 0.0 && v1_magnitude != 0.0) { vector[0].normalize(); vector[1].normalize(); float sign_f = sign ? -1f : 1f; double x = (double)vector[0].cross(vector[1]).magnitude(); double y = (double)vector[0].dot(vector[1]); float theta = (float)Math.Atan2(sign_f * x, sign_f * y); return theta;// -theta; } else { return 0.0f; } }
public static float getAngleXY(SkeletonPoint a, SkeletonPoint b, SkeletonPoint c) { Vecto3Float[] vector = new Vecto3Float[2]; vector[0] = new Vecto3Float(c.X - b.X, c.Y - b.Y, 0); vector[1] = new Vecto3Float(a.X - b.X, a.Y - b.Y, 0); float v0_magnitude = vector[0].magnitude(); float v1_magnitude = vector[1].magnitude(); if (v0_magnitude != 0.0 && v1_magnitude != 0.0) { vector[0].normalize(); vector[1].normalize(); double x = (double)vector[0].cross(vector[1]).magnitude(); double y = (double)vector[0].dot(vector[1]); float theta = (float)Math.Atan2(x, y); return(-theta); } else { return(0.0f); } }
///<summary> /// Receive a rotation matrix of type Matrix4 /// Return a Vector3Float containing the rotation around the 3 axis ///</summary> public static Vecto3Float computeEulerFromMatrixXYZ(Matrix3x3 m) { Vecto3Float sol = new Vecto3Float(); if (m.matrix[0, 2] < 1f) { if (m.matrix[0, 2] > -1f) { sol.X = (float)Math.Atan2(-m.matrix[1, 2], m.matrix[2, 2]); sol.Y = (float)Math.Asin(-(m.matrix[0, 2])); sol.Z = (float)Math.Atan2(-m.matrix[0, 1], m.matrix[0, 0]); } else { sol.X = -(float)Math.Atan2(m.matrix[1, 0], m.matrix[1, 1]); sol.Y = -(float)(Math.PI) / 2; sol.Z = 0f; } } else { sol.X = (float)Math.Atan2(m.matrix[1, 0], m.matrix[1, 1]); sol.Y = (float)(Math.PI) / 2; sol.Z = 0f; } return(sol); }
public static float getAngleZY(SkeletonPoint a, SkeletonPoint b, SkeletonPoint c) { Vecto3Float[] vector = new Vecto3Float[2]; //vector[0] = new Vecto3Float(0, b.Y - c.Y, b.Z - c.Z); vector[0] = new Vecto3Float(0, c.Y - b.Y, c.Z - b.Z); vector[1] = new Vecto3Float(0, a.Y - b.Y, a.Z - b.Z); float v0_magnitude = vector[0].magnitude(); float v1_magnitude = vector[1].magnitude(); if (v0_magnitude != 0.0 && v1_magnitude != 0.0) { vector[0].normalize(); vector[1].normalize(); double x = (double)vector[0].cross(vector[1]).magnitude(); double y = (double)vector[0].dot(vector[1]); float theta = (float)Math.Atan2(x, y); float theta2 = (float)Math.Atan2(-x, y); float theta3 = (float)Math.Atan2(x, -y); float theta4 = (float)Math.Atan2(-x, -y); return(-theta); } else { return(0.0f); } }
///<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> /// Receive three skeleton point (3D position of a joint). /// Compute two vector, return the angle between them. /// If something is wrong return 0 ///</summary> public static float getAngle(SkeletonPoint a, SkeletonPoint b, SkeletonPoint c, bool sign) { Vecto3Float[] vector = new Vecto3Float[2]; //vector[0] = new Vecto3Float(b.X - c.X, b.Y - c.Y, b.Z - c.Z); vector[0] = new Vecto3Float(c.X - b.X, c.Y - b.Y, c.Z - b.Z); vector[1] = new Vecto3Float(a.X - b.X, a.Y - b.Y, a.Z - b.Z); float v0_magnitude = vector[0].magnitude(); float v1_magnitude = vector[1].magnitude(); if (v0_magnitude != 0.0 && v1_magnitude != 0.0) { vector[0].normalize(); vector[1].normalize(); float sign_f = sign ? -1f : 1f; double x = (double)vector[0].cross(vector[1]).magnitude(); double y = (double)vector[0].dot(vector[1]); float theta = (float)Math.Atan2(sign_f * x, sign_f * y); return(theta);// -theta; } else { return(0.0f); } }
public Vecto3Float cross(Vecto3Float vec) { Vecto3Float res = new Vecto3Float(); res.X = this.Y * vec.Z - vec.Y * this.Z; res.Y = this.Z * vec.X - vec.Z * this.X; res.Z = this.X * vec.Y - vec.Z * this.Y; return(res); }
public Vecto3Float cross(Vecto3Float vec) { Vecto3Float res = new Vecto3Float(); res.X = this.Y * vec.Z - vec.Y * this.Z; res.Y = this.Z * vec.X - vec.Z * this.X; res.Z = this.X * vec.Y - vec.Z * this.Y; return res; }
///<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; }
public static float getAngleNew(Vecto3Float vect1, Vecto3Float vect2) { vect1.normalize(); vect2.normalize(); if (vect1.magnitude() != 0.0 && vect2.magnitude() != 0.0) { double x = (double)vect1.cross(vect2).magnitude(); double y = (double)vect1.dot(vect2); return atan2(x, y); } else { return 0f; } }
public static float getAngleNew(Vecto3Float vect1, Vecto3Float vect2) { vect1.normalize(); vect2.normalize(); if (vect1.magnitude() != 0.0 && vect2.magnitude() != 0.0) { double x = (double)vect1.cross(vect2).magnitude(); double y = (double)vect1.dot(vect2); return(atan2(x, y)); } else { return(0f); } }
///<summary> /// Compute angle between the joint according to the position of the joint ///</summary> private void angleVector() { SkeletonPoint shoulderLeft = this.avgPoint[JointType.ShoulderLeft]; SkeletonPoint shoulderCenter = this.avgPoint[JointType.ShoulderCenter]; SkeletonPoint shoulderRight = this.avgPoint[JointType.ShoulderRight]; SkeletonPoint elbowLeft = this.avgPoint[JointType.ElbowLeft]; SkeletonPoint elbowRight = this.avgPoint[JointType.ElbowRight]; SkeletonPoint wristLeft = this.avgPoint[JointType.WristLeft]; SkeletonPoint wristRight = this.avgPoint[JointType.WristRight]; SkeletonPoint hipLeft = this.avgPoint[JointType.HipLeft]; SkeletonPoint hipCenter = this.avgPoint[JointType.HipCenter]; SkeletonPoint hipRight = this.avgPoint[JointType.HipRight]; Vecto3Float upperArmLeft = new Vecto3Float(shoulderLeft.X - elbowLeft.X, shoulderLeft.Y - elbowLeft.Y, shoulderLeft.Z - elbowLeft.Z); Vecto3Float lowerArmLeft = new Vecto3Float(wristLeft.X - elbowLeft.X, wristLeft.Y - elbowLeft.Y, wristLeft.Z - elbowLeft.Z); Vecto3Float upperArmRight = new Vecto3Float(shoulderRight.X - elbowRight.X, shoulderRight.Y - elbowRight.Y, shoulderRight.Z - elbowRight.Z); Vecto3Float lowerArmRight = new Vecto3Float(wristRight.X - elbowRight.X, wristRight.Y - elbowRight.Y, wristRight.Z - elbowRight.Z); Vecto3Float shoulderLToHipLeft = new Vecto3Float(shoulderLeft.X - hipLeft.X, shoulderLeft.Y - hipLeft.Y, shoulderLeft.Z - hipLeft.Z); Vecto3Float shoulderRToHipRight = new Vecto3Float(shoulderRight.X - hipRight.X, shoulderRight.Y - hipRight.Y, shoulderRight.Z - hipRight.Z); Vecto3Float shoulderLToShoulderC = new Vecto3Float(shoulderLeft.X - shoulderCenter.X, shoulderLeft.Y - shoulderCenter.Y, shoulderLeft.Z - shoulderCenter.Z); Vecto3Float shoulderRToShoulderC = new Vecto3Float(shoulderRight.X - shoulderCenter.X, shoulderRight.Y - shoulderCenter.Y, shoulderRight.Z - shoulderCenter.Z); if (hipLeft.Z + 0.15 > elbowLeft.Z) //if the elbow is in front of the hip { //Shoulder Roll //Left if (Math.Abs(shoulderLeft.Z - elbowLeft.Z) < 0.1 && Math.Abs(shoulderLeft.X - elbowLeft.X) < 0.1) { this.jointAngles[NAOConversion.LShoulderRoll] = NAOConversion.convertAngle(NAOConversion.LShoulderRoll, Kinematic.getAngleNew(upperArmLeft.projectionOntoXY(), shoulderLToHipLeft.projectionOntoXY())); } else { this.jointAngles[NAOConversion.LShoulderRoll] = NAOConversion.convertAngle(NAOConversion.LShoulderRoll, Kinematic.getAngleNew(upperArmLeft.projectionOntoZX(), shoulderLToHipLeft.projectionOntoZX())); } //Shoulder Pitch //Left this.jointAngles[NAOConversion.LShoulderPitch] = NAOConversion.convertAngle(NAOConversion.LShoulderPitch, Kinematic.getAngleNew(upperArmLeft.projectionOntoZY(), shoulderLToHipLeft.projectionOntoZY())); } if (hipRight.Z + 0.15 > elbowRight.Z)//if the elbow is in front of the hip { //Shoulder Roll //Right if (Math.Abs(shoulderRight.Z - elbowRight.Z) < 0.1 && Math.Abs(shoulderRight.X - elbowRight.X) < 0.1) { this.jointAngles[NAOConversion.RShoulderRoll] = NAOConversion.convertAngle(NAOConversion.RShoulderRoll, Kinematic.getAngleNew(upperArmRight.projectionOntoXY(), shoulderRToHipRight.projectionOntoXY())); } else { this.jointAngles[NAOConversion.RShoulderRoll] = NAOConversion.convertAngle(NAOConversion.RShoulderRoll, Kinematic.getAngleNew(upperArmRight.projectionOntoZX(), shoulderRToHipRight.projectionOntoZX())); } //Shoulder Pitch //Right this.jointAngles[NAOConversion.RShoulderPitch] = NAOConversion.convertAngle(NAOConversion.RShoulderPitch, Kinematic.getAngleNew(shoulderRToHipRight.projectionOntoZY(), upperArmRight.projectionOntoZY())); } //Elbow Roll //Left this.jointAngles[NAOConversion.LElbowRoll] = NAOConversion.convertAngle(NAOConversion.LElbowRoll, Kinematic.getAngleNew(upperArmLeft, lowerArmLeft)); //Right this.jointAngles[NAOConversion.RElbowRoll] = NAOConversion.convertAngle(NAOConversion.RElbowRoll, Kinematic.getAngleNew(upperArmRight, lowerArmRight)); //Elbow Yaw ////Left Vecto3Float shoulderLTohipCenter = new Vecto3Float(shoulderLeft.X - hipCenter.X, shoulderLeft.Y - hipCenter.Y, shoulderLeft.Z - hipCenter.Z); Vecto3Float lCrossCenterArm = upperArmLeft.cross(shoulderLTohipCenter); Vecto3Float lCrossArms = upperArmLeft.cross(lowerArmLeft); float lElbowYaw = Kinematic.getAngleNew(lCrossCenterArm, lCrossArms); this.jointAngles[NAOConversion.LElbowYaw] = NAOConversion.convertAngle(NAOConversion.LElbowYaw, lElbowYaw); //Right Vecto3Float shoulderRTohipCenter = new Vecto3Float(shoulderRight.X - hipCenter.X, shoulderRight.Y - hipCenter.Y, shoulderRight.Z - hipCenter.Z); Vecto3Float rCrossCenterArm = upperArmRight.cross(shoulderRTohipCenter); Vecto3Float rCrossArms = upperArmRight.cross(lowerArmRight); float rElbowYaw = Kinematic.getAngleNew(rCrossCenterArm, rCrossArms); this.jointAngles[NAOConversion.RElbowYaw] = NAOConversion.convertAngle(NAOConversion.RElbowYaw, rElbowYaw); }
public static float getAngleXY(SkeletonPoint a, SkeletonPoint b, SkeletonPoint c) { Vecto3Float[] vector = new Vecto3Float[2]; vector[0] = new Vecto3Float(c.X - b.X, c.Y - b.Y, 0); vector[1] = new Vecto3Float(a.X - b.X, a.Y - b.Y, 0); float v0_magnitude = vector[0].magnitude(); float v1_magnitude = vector[1].magnitude(); if (v0_magnitude != 0.0 && v1_magnitude != 0.0) { vector[0].normalize(); vector[1].normalize(); double x = (double)vector[0].cross(vector[1]).magnitude(); double y = (double)vector[0].dot(vector[1]); float theta = (float)Math.Atan2(x, y); return -theta; } else { return 0.0f; } }
public static float getAngleZY(SkeletonPoint a, SkeletonPoint b, SkeletonPoint c) { Vecto3Float[] vector = new Vecto3Float[2]; //vector[0] = new Vecto3Float(0, b.Y - c.Y, b.Z - c.Z); vector[0] = new Vecto3Float(0, c.Y - b.Y, c.Z - b.Z); vector[1] = new Vecto3Float(0, a.Y - b.Y, a.Z - b.Z); float v0_magnitude = vector[0].magnitude(); float v1_magnitude = vector[1].magnitude(); if (v0_magnitude != 0.0 && v1_magnitude != 0.0) { vector[0].normalize(); vector[1].normalize(); double x = (double)vector[0].cross(vector[1]).magnitude(); double y = (double)vector[0].dot(vector[1]); float theta = (float)Math.Atan2(x, y); float theta2 = (float)Math.Atan2(-x, y); float theta3 = (float)Math.Atan2(x, -y); float theta4 = (float)Math.Atan2(-x, -y); return -theta; } else { return 0.0f; } }
public float dot(Vecto3Float vect) { return this.x * vect.X + this.y * vect.Y + this.z * vect.Z; }
public float dot(Vecto3Float vect) { return(this.x * vect.X + this.y * vect.Y + this.z * vect.Z); }