inlib
1.2.0
|
00001 // Copyright (C) 2010, Guy Barrand. All rights reserved. 00002 // See the file inlib.license for terms. 00003 00004 #ifndef inlib_qrot 00005 #define inlib_qrot 00006 00007 // rotation done with quaternion. 00008 00009 #include "a4" 00010 #include "a3" 00011 00012 namespace inlib { 00013 00014 template <class T> 00015 class qrot { 00016 public: 00017 qrot() 00018 :m_quat(0,0,0,1) //zero rotation around the positive Z axis. 00019 {} 00020 qrot(const a3::vec<T>& a_axis,T a_radians){ 00021 if(!set_value(a_axis,a_radians)) {} //FIXME : throw 00022 } 00023 //qrot(const a4::sqm<T>& a_matrix){ 00024 // set_value(a_matrix); 00025 //} 00026 virtual ~qrot(){} 00027 public: 00028 qrot(const qrot& a_from) 00029 :m_quat(a_from.m_quat) 00030 {} 00031 qrot& operator=(const qrot& a_from){ 00032 m_quat = a_from.m_quat; 00033 return *this; 00034 } 00035 protected: 00036 qrot(T a_q0,T a_q1,T a_q2,T a_q3) 00037 :m_quat(a_q0,a_q1,a_q2,a_q3) 00038 { 00039 if(!m_quat.normalize()) {} //FIXME throw 00040 } 00041 00042 public: 00043 qrot& operator*=(const qrot& a_q) { 00044 //Multiplies the quaternions. 00045 //Note that order is important when combining quaternions with the 00046 //multiplication operator. 00047 // Formula from <http://www.lboro.ac.uk/departments/ma/gallery/quat/> 00048 00049 T tx = m_quat.v0(); 00050 T ty = m_quat.v1(); 00051 T tz = m_quat.v2(); 00052 T tw = m_quat.v3(); 00053 00054 T qx = a_q.m_quat.v0(); 00055 T qy = a_q.m_quat.v1(); 00056 T qz = a_q.m_quat.v2(); 00057 T qw = a_q.m_quat.v3(); 00058 00059 m_quat.set_value(qw*tx + qx*tw + qy*tz - qz*ty, 00060 qw*ty - qx*tz + qy*tw + qz*tx, 00061 qw*tz + qx*ty - qy*tx + qz*tw, 00062 qw*tw - qx*tx - qy*ty - qz*tz); 00063 m_quat.normalize(); 00064 return *this; 00065 } 00066 00067 bool operator==(const qrot& a_r) const { 00068 return m_quat.equal(a_r.m_quat); 00069 } 00070 bool operator!=(const qrot& a_r) const { 00071 return !operator==(a_r); 00072 } 00073 qrot operator*(const qrot& a_r) const { 00074 qrot tmp(*this); 00075 tmp *= a_r; 00076 return tmp; 00077 } 00078 00079 bool invert(){ 00080 T length = m_quat.length(); 00081 if(length==T()) return false; 00082 00083 // Optimize by doing 1 div and 4 muls instead of 4 divs. 00084 T inv = one() / length; 00085 00086 m_quat.set_value(-m_quat.v0() * inv, 00087 -m_quat.v1() * inv, 00088 -m_quat.v2() * inv, 00089 m_quat.v3() * inv); 00090 00091 return true; 00092 } 00093 00094 bool inverse(qrot& a_r) const { 00095 //Non-destructively inverses the rotation and returns the result. 00096 T length = m_quat.length(); 00097 if(length==T()) return false; 00098 00099 // Optimize by doing 1 div and 4 muls instead of 4 divs. 00100 T inv = one() / length; 00101 00102 a_r.m_quat.set_value(-m_quat.v0() * inv, 00103 -m_quat.v1() * inv, 00104 -m_quat.v2() * inv, 00105 m_quat.v3() * inv); 00106 00107 return true; 00108 } 00109 00110 00111 bool set_value(const a3::vec<T>& a_axis,T a_radians) { 00112 // Reset rotation with the given axis-of-rotation and rotation angle. 00113 // Make sure axis is not the null vector when calling this method. 00114 // From <http://www.automation.hut.fi/~jaro/thesis/hyper/node9.html>. 00115 if(a_axis.length()==T()) return false; 00116 m_quat.v3(::cos(a_radians/2)); 00117 T sineval = ::sin(a_radians/2); 00118 a3::vec<T> a = a_axis; 00119 a.normalize(); 00120 m_quat.v0(a.v0() * sineval); 00121 m_quat.v1(a.v1() * sineval); 00122 m_quat.v2(a.v2() * sineval); 00123 return true; 00124 } 00125 00126 bool value(a3::vec<T>& a_axis,T& a_radians) const { 00127 //WARNING : can fail. 00128 if( (m_quat.v3()<minus_one()) || (m_quat.v3()> one()) ){ 00129 a_axis.set_value(0,0,1); 00130 a_radians = 0; 00131 return false; 00132 } 00133 00134 a_radians = ::acos(m_quat.v3()) * 2; 00135 T sineval = ::sin(a_radians/2); 00136 00137 if(sineval==T()) { //??? 00138 a_axis.set_value(0,0,1); 00139 a_radians = 0; 00140 return false; 00141 } 00142 a_axis.set_value(m_quat.v0()/sineval, 00143 m_quat.v1()/sineval, 00144 m_quat.v2()/sineval); 00145 return true; 00146 } 00147 00148 /* 00149 void set_value(const a4::sqm<T>& a_m) { 00150 //Set the rotation from the components of the given matrix. 00151 T scalerow = a_m.value(0,0) + a_m.value(1,1) + a_m.value(2,2); 00152 if (scalerow > T()) { 00153 T s = fsqrt(scalerow + a_m.value(3,3)); 00154 m_quat.v3(s * 0.5f); 00155 s = 0.5f / s; 00156 00157 m_quat.v0((a_m.value(1,2) - a_m.value(2,1)) * s); 00158 m_quat.v1((a_m.value(2,0) - a_m.value(0,2)) * s); 00159 m_quat.v2((a_m.value(0,1) - a_m.value(1,0)) * s); 00160 } else { 00161 unsigned int i = 0; 00162 if (a_m.value(1,1) > a_m.value(0,0)) i = 1; 00163 if (a_m.value(2,2) > a_m.value(i,i)) i = 2; 00164 00165 unsigned int j = (i+1)%3; 00166 unsigned int k = (j+1)%3; 00167 00168 T s = fsqrt((a_m.value(i,i) - (a_m.value(j,j) + a_m.value(k,k))) + a_m.value(3,3)); 00169 00170 m_quat.set_value(i,s * 0.5f); 00171 s = 0.5f / s; 00172 00173 m_quat.v3((a_m.value(j,k) - a_m.value(k,j)) * s); 00174 m_quat.set_value(j,(a_m.value(i,j) + a_m.value(j,i)) * s); 00175 m_quat.set_value(k,(a_m.value(i,k) + a_m.value(k,i)) * s); 00176 } 00177 00178 if (a_m.value(3,3)!=1.0f) { 00179 m_quat.multiply(1.0f/fsqrt(a_m.value(3,3))); 00180 } 00181 } 00182 */ 00183 00184 void value(a4::sqm<T>& matrix) const { 00185 //Return this rotation in the form of a matrix. 00186 const T x = m_quat.v0(); 00187 const T y = m_quat.v1(); 00188 const T z = m_quat.v2(); 00189 const T w = m_quat.v3(); 00190 // z = w + x * i + y * j + z * k 00191 00192 // first row : 00193 matrix.set_value(0,0,w*w + x*x - y*y - z*z); 00194 matrix.set_value(0,1,2*x*y - 2*w*z); 00195 matrix.set_value(0,2,2*x*z + 2*w*y); 00196 matrix.set_value(0,3,0); 00197 00198 // second row : 00199 matrix.set_value(1,0,2*x*y + 2*w*z); 00200 matrix.set_value(1,1,w*w - x*x + y*y - z*z); 00201 matrix.set_value(1,2,2*y*z - 2*w*x); 00202 matrix.set_value(1,3,0); 00203 00204 // third row : 00205 matrix.set_value(2,0,2*x*z - 2*w*y); 00206 matrix.set_value(2,1,2*y*z + 2*w*x); 00207 matrix.set_value(2,2,w*w - x*x - y*y + z*z); 00208 matrix.set_value(2,3,0); 00209 00210 // fourth row : 00211 matrix.set_value(3,0,0); 00212 matrix.set_value(3,1,0); 00213 matrix.set_value(3,2,0); 00214 matrix.set_value(3,3,w*w + x*x + y*y + z*z); 00215 } 00216 00217 void mult_vec(const a3::vec<T>& a_in,a3::vec<T>& a_out) const { 00218 const T x = m_quat.v0(); 00219 const T y = m_quat.v1(); 00220 const T z = m_quat.v2(); 00221 const T w = m_quat.v3(); 00222 00223 // first row : 00224 T v0 = (w*w + x*x - y*y - z*z) * a_in.v0() 00225 + (2*x*y - 2*w*z) * a_in.v1() 00226 + (2*x*z + 2*w*y) * a_in.v2(); 00227 00228 T v1 = (2*x*y + 2*w*z) * a_in.v0() 00229 +(w*w - x*x + y*y - z*z) * a_in.v1() 00230 + (2*y*z - 2*w*x) * a_in.v2(); 00231 00232 T v2 = (2*x*z - 2*w*y) * a_in.v0() 00233 + (2*y*z + 2*w*x) * a_in.v1() 00234 +(w*w - x*x - y*y + z*z) * a_in.v2(); 00235 00236 a_out.set_value(v0,v1,v2); 00237 } 00238 00239 public: //for io::streamer 00240 const a4::vec<T>& quat() const {return m_quat;} 00241 a4::vec<T>& quat() {return m_quat;} 00242 00243 protected: 00244 static T one() {return T(1);} 00245 static T minus_one() {return T(-1);} 00246 00247 protected: 00248 a4::vec<T> m_quat; 00249 00250 public: 00251 //NOTE : don't handle a static object because of mem balance. 00252 //static const qrot<double>& identity() { 00253 // static const qrot<double> s_v(0,0,0,1); 00254 // return s_v; 00255 //} 00256 00257 }; 00258 00259 } 00260 00261 #endif