00001
00002
00003
00004
00005
00006
00007
00008
00009
00010
00011
00012
00013
00014
00015
00016
00017
00018
00019
00020
00021
00022
00023
00024 #include "ExQuaternion.h"
00025
00026 ExQuaternion::ExQuaternion()
00027 {
00028 SetQuaternion(1,0,0,0);
00029 }
00030
00031 ExQuaternion::ExQuaternion(float w,float x,float y,float z)
00032 {
00033 SetQuaternion(w,x,y,z);
00034 }
00035
00036 ExQuaternion::~ExQuaternion()
00037 {
00038
00039 }
00040
00042
00044
00045 {
00046 qw=w;
00047 qx=x;
00048 qy=y;
00049 qz=z;
00050 }
00051
00052 void ExQuaternion::Conjugate(void)
00053 {
00054
00055 }
00056
00057 void ExQuaternion::LoadIdentity(void)
00058 {
00059 qx=-qx;
00060 qy=-qy;
00061 qz=-qz;
00062 }
00063 float ExQuaternion::GetMagnitude(void)
00064 {
00065 return(sqrt(qw*qw+qx*qx+qy*qy+qz*qz));
00066
00067 }
00068 void ExQuaternion::Normalize(void)
00069 {
00070 float factor=GetMagnitude();
00071 float scaleby(1.0/sqrt(factor));
00072 qw=qw*scaleby;
00073 qx=qx*scaleby;
00074 qy=qy*scaleby;
00075 qz=qz*scaleby;
00076 }
00077
00078 void ExQuaternion::SetEuler(float yaw,float pitch,float roll)
00079 {
00080 float cosY = cosf(yaw/2.0f);
00081 float sinY = sinf(yaw/2.0f);
00082 float cosP = cosf(pitch/2.0f);
00083 float sinP = sinf(pitch/2.0f);
00084 float cosR = cosf(roll/2.0f);
00085 float sinR = sinf(roll/2.0f);
00086
00087 qw=cosR*sinP*cosY+sinR*cosP*sinY;
00088 qx=cosR*cosP*sinY-sinR*sinP*cosY;
00089 qy=sinR*cosP*cosY-cosR*sinP*sinY;
00090 qz=cosR*cosP*cosY+sinR*sinP*sinY;
00091
00092 }
00094
00096
00097 {
00098 ExQuaternion result;
00099 result.qw=(Q.qw*result.qw)-(Q.qx*result.qx)-(Q.qy*result.qy)-(Q.qz*result.qz);
00100 result.qx=(Q.qw*result.qx)-(Q.qx*result.qw)-(Q.qy*result.qz)-(Q.qz*result.qw);
00101 result.qy=(Q.qw*result.qy)-(Q.qx*result.qz)-(Q.qy*result.qw)-(Q.qz*result.qx);
00102 result.qz=(Q.qw*result.qz)-(Q.qx*result.qy)-(Q.qy*result.qx)-(Q.qz*result.qw);
00103 return result;
00104 }
00106
00108
00109 std::ostream& operator<<(std::ostream& s,const ExQuaternion &q)
00110 {
00111 Guard(friend std::ostream& operator<<(std::ostream& s,const ExQuaternion &q))
00112 s<<"W:"<<q.qw<<"X:"<<q.qx<<"Y:"<<q.qy<<"Z:"<<q.qz<<std::endl;
00113 return s;
00114 UnGuard
00115 }