Quaternion2Vector3

publicstaticQuaternionToQ(Vector3v){returnToQ(v.y,v.x,v.z);}publicstaticQuaternionToQ(floatyaw,floatpitch,floatroll){yaw*=Mathf.Deg2Rad;pitch*=Mathf.Deg2Rad;roll*=Mathf.Deg2Rad;floatrollOver2=roll*0.5f;floatsinRollOver2=(float)Math.Sin((double)rollOver2);floatcosRollOver2=(float)Math.Cos((double)rollOver2);floatpitchOver2=pitch*0.5f;floatsinPitchOver2=(float)Math.Sin((double)pitchOver2);floatcosPitchOver2=(float)Math.Cos((double)pitchOver2);floatyawOver2=yaw*0.5f;floatsinYawOver2=(float)Math.Sin((double)yawOver2);floatcosYawOver2=(float)Math.Cos((double)yawOver2);Quaternionresult;result.w=cosYawOver2*cosPitchOver2*cosRollOver2+sinYawOver2*sinPitchOver2*sinRollOver2;result.x=cosYawOver2*sinPitchOver2*cosRollOver2+sinYawOver2*cosPitchOver2*sinRollOver2;result.y=sinYawOver2*cosPitchOver2*cosRollOver2-cosYawOver2*sinPitchOver2*sinRollOver2;result.z=cosYawOver2*cosPitchOver2*sinRollOver2-sinYawOver2*sinPitchOver2*cosRollOver2;returnresult;}publicstaticVector3FromQ2(Quaternionq1){floatsqw=q1.w*q1.w;floatsqx=q1.x*q1.x;floatsqy=q1.y*q1.y;floatsqz=q1.z*q1.z;floatunit=sqx+sqy+sqz+sqw;// if normalised is one, otherwise is correction factorfloattest=q1.x*q1.w-q1.y*q1.z;Vector3v;if(test>0.4995f*unit){// singularity at north polev.y=2f*Mathf.Atan2(q1.y,q1.x);v.x=Mathf.PI/2;v.z=0;returnNormalizeAngles(v*Mathf.Rad2Deg);}if(test<-0.4995f*unit){// singularity at south polev.y=-2f*Mathf.Atan2(q1.y,q1.x);v.x=-Mathf.PI/2;v.z=0;returnNormalizeAngles(v*Mathf.Rad2Deg);}Quaternionq=newQuaternion(q1.w,q1.z,q1.x,q1.y);v.y=(float)Math.Atan2(2f*q.x*q.w+2f*q.y*q.z,1-2f*(q.z*q.z+q.w*q.w));// Yawv.x=(float)Math.Asin(2f*(q.x*q.z-q.w*q.y));// Pitchv.z=(float)Math.Atan2(2f*q.x*q.y+2f*q.z*q.w,1-2f*(q.y*q.y+q.z*q.z));// RollreturnNormalizeAngles(v*Mathf.Rad2Deg);}staticVector3NormalizeAngles(Vector3angles){angles.x=NormalizeAngle(angles.x);angles.y=NormalizeAngle(angles.y);angles.z=NormalizeAngle(angles.z);returnangles;}staticfloatNormalizeAngle(floatangle){while(angle>360)angle-=360;while(angle<0)angle+=360;returnangle;}