tgQuaternion.cpp
Go to the documentation of this file.
00001 
00002 #include <blort/TomGine/tgQuaternion.h>
00003 #include <stdio.h>
00004 
00005 using namespace TomGine;
00006 
00007 tgQuaternion::tgQuaternion(){
00008         x=0.0;
00009         y=0.0;
00010         z=0.0;
00011         w=1.0;
00012 }
00013 
00014 tgQuaternion::tgQuaternion(float x, float y, float z, float w){
00015         this->x = x;
00016         this->y = y;
00017         this->z = z;
00018         this->w = w;
00019 }
00020 
00021 // normalising a quaternion works similar to a vector. This method will not do anything
00022 // if the quaternion is close enough to being unit-length. define TOLERANCE as something
00023 // small like 0.00001f to get accurate results
00024 void tgQuaternion::normalise(){
00025         // Don't normalize if we don't have to
00026         float mag2 = w * w + x * x + y * y + z * z;
00027         if (fabs(mag2 - 1.0f) > ftol) {
00028                 float mag = sqrt(mag2);
00029                 w /= mag;
00030                 x /= mag;
00031                 y /= mag;
00032                 z /= mag;
00033         }
00034 }
00035 
00036 // We need to get the inverse of a quaternion to properly apply a quaternion-rotation to a vector
00037 // The conjugate of a quaternion is the same as the inverse, as long as the quaternion is unit-length
00038 tgQuaternion tgQuaternion::getConjugate() const{
00039         return tgQuaternion(-x, -y, -z, w);
00040 }
00041 
00042 // Adding
00043 tgQuaternion tgQuaternion::operator+ (const tgQuaternion &q2) const{
00044         tgQuaternion rq;
00045         rq.x = x+q2.x;
00046         rq.y = y+q2.y;
00047         rq.z = z+q2.z;
00048         rq.w = w+q2.w;
00049         
00050         return rq;
00051 }
00052 
00053 // Subtracting
00054 tgQuaternion tgQuaternion::operator- (const tgQuaternion &q2) const{
00055         tgQuaternion rq;
00056         rq.x = x-q2.x;
00057         rq.y = y-q2.y;
00058         rq.z = z-q2.z;
00059         rq.w = w-q2.w;
00060         return rq;
00061 }
00062 
00063 
00064 // Multiplying q1 with q2 applies the rotation q2 to q1
00065 tgQuaternion tgQuaternion::operator* (const tgQuaternion &rq){
00066         // the constructor takes its arguments as (x, y, z, w)
00067         return tgQuaternion(w * rq.x + x * rq.w + y * rq.z - z * rq.y,
00068                           w * rq.y + y * rq.w + z * rq.x - x * rq.z,
00069                           w * rq.z + z * rq.w + x * rq.y - y * rq.x,
00070                           w * rq.w - x * rq.x - y * rq.y - z * rq.z);
00071 }
00072 
00073 tgQuaternion tgQuaternion::operator* (const float f){
00074         return tgQuaternion(x*f, y*f, z*f, w*f);
00075 }
00076 
00077 
00078 // Multiplying a quaternion q with a vector v applies the q-rotation to v
00079 vec3 tgQuaternion::operator* (const vec3 &vec){
00080         vec3 vn(vec);
00081         vn.normalize();
00082  
00083         tgQuaternion vecQuat, resQuat;
00084         vecQuat.x = vn.x;
00085         vecQuat.y = vn.y;
00086         vecQuat.z = vn.z;
00087         vecQuat.w = 0.0f;
00088  
00089         resQuat = vecQuat * getConjugate();
00090         resQuat = *this * resQuat;
00091  
00092         return (vec3(resQuat.x, resQuat.y, resQuat.z));
00093 }
00094 
00095 // Convert from Axis Angle
00096 void tgQuaternion::fromAxis(const vec3 &v, float angle){
00097         float sinAngle;
00098         angle *= 0.5f;
00099         vec3 vn(v);
00100         vn.normalize();
00101  
00102         sinAngle = sin(angle);
00103  
00104         x = (vn.x * sinAngle);
00105         y = (vn.y * sinAngle);
00106         z = (vn.z * sinAngle);
00107         w = cos(angle);
00108 }
00109 
00110 // Convert from Euler Angles
00111 void tgQuaternion::fromEuler(float roll, float pitch, float yaw){
00112         // Basically we create 3 tgQuaternions, one for pitch, one for yaw, one for roll
00113         // and multiply those together.
00114         // the calculation below does the same, just shorter
00115  
00116         float p = pitch / 2.0f;
00117         float y = yaw / 2.0f;
00118         float r = roll / 2.0f;
00119  
00120         float sinp = sin(p);
00121         float siny = sin(y);
00122         float sinr = sin(r);
00123         float cosp = cos(p);
00124         float cosy = cos(y);
00125         float cosr = cos(r);
00126  
00127         this->x = sinr * cosp * cosy - cosr * sinp * siny;
00128         this->y = cosr * sinp * cosy + sinr * cosp * siny;
00129         this->z = cosr * cosp * siny - sinr * sinp * cosy;
00130         this->w = cosr * cosp * cosy + sinr * sinp * siny;
00131  
00132         normalise();
00133 }
00134 
00135 // Convert from Matrix 4x4
00136 void tgQuaternion::fromMatrix(mat4 m){
00137         w = sqrt(1.0f + m[0] + m[5] + m[10]) / 2.0f;
00138         float w4 = (4.0f * w);
00139         x = (m[9] - m[6]) / w4 ;
00140         y = (m[2] - m[8]) / w4 ;
00141         z = (m[4] - m[1]) / w4 ;
00142         
00143         normalise();
00144 }
00145 
00146 // Convert from Matrix 3x3
00147 void tgQuaternion::fromMatrix(mat3 m){
00148         w = sqrt(1.0f + m[0] + m[4] + m[8]) / 2.0f;
00149         float w4 = (4.0f * w);
00150         x = (m[7] - m[5]) / w4 ;
00151         y = (m[2] - m[6]) / w4 ;
00152         z = (m[3] - m[1]) / w4 ;
00153         
00154         normalise();
00155 }
00156 
00157 // Convert to Matrix 4x4
00158 mat4 tgQuaternion::getMatrix4() const{
00159         float x2 = x * x;
00160         float y2 = y * y;
00161         float z2 = z * z;
00162         float xy = x * y;
00163         float xz = x * z;
00164         float yz = y * z;
00165         float wx = w * x;
00166         float wy = w * y;
00167         float wz = w * z;
00168  
00169         mat4 rot;
00170         rot[0]=1.0f - 2.0f * (y2 + z2);
00171         rot[1]=2.0f * (xy - wz);
00172         rot[2]=2.0f * (xz + wy);
00173         rot[3]=0.0f;
00174         rot[4]=2.0f * (xy + wz);
00175         rot[5]=1.0f - 2.0f * (x2 + z2);
00176         rot[6]=2.0f * (yz - wx);
00177         rot[7]=0.0f;
00178         rot[8]=2.0f * (xz - wy);
00179         rot[9]=2.0f * (yz + wx);
00180         rot[10]=1.0f - 2.0f * (x2 + y2);
00181         rot[11]=0.0f;
00182         rot[12]=0.0f;
00183         rot[13]=0.0f;
00184         rot[14]=0.0f;
00185         rot[15]=1.0f;
00186         
00187         return rot;
00188 }
00189 
00190 // Convert to Matrix 3x3
00191 mat3 tgQuaternion::getMatrix3() const{
00192         float x2 = x * x;
00193         float y2 = y * y;
00194         float z2 = z * z;
00195         float xy = x * y;
00196         float xz = x * z;
00197         float yz = y * z;
00198         float wx = w * x;
00199         float wy = w * y;
00200         float wz = w * z;
00201         
00202         mat3 rot;
00203         rot[0]=1.0f - 2.0f * (y2 + z2);
00204         rot[1]=2.0f * (xy - wz);
00205         rot[2]=2.0f * (xz + wy);
00206         rot[3]=2.0f * (xy + wz);
00207         rot[4]=1.0f - 2.0f * (x2 + z2);
00208         rot[5]=2.0f * (yz - wx);
00209         rot[6]=2.0f * (xz - wy);
00210         rot[7]=2.0f * (yz + wx);
00211         rot[8]=1.0f - 2.0f * (x2 + y2);
00212         
00213         return rot;
00214 }
00215 
00216 
00217 // Convert to Axis/Angles
00218 void tgQuaternion::getAxisAngle(vec3& axis, float& angle) const{
00219         float scale = sqrt(x * x + y * y + z * z);
00220         axis.x = x / scale;
00221         axis.y = y / scale;
00222         axis.z = z / scale;
00223         angle = acos(w) * 2.0f;
00224         if(angle < 0.0 || angle > (2.0*PI))
00225                 angle = 0.0;
00226 }
00227 
00228 void tgQuaternion::print() const{
00229   printf("x: %f y: %f z: %f w: %f\n", x, y, z, w);
00230 }


blort
Author(s): Michael Zillich, Thomas Mörwald, Johann Prankl, Andreas Richtsfeld, Bence Magyar (ROS version)
autogenerated on Thu Jan 2 2014 11:38:26