Warning, /include/Geant4/tools/lina/qrot is written in an unsupported language. File is not indexed.
0001 // Copyright (C) 2010, Guy Barrand. All rights reserved.
0002 // See the file tools.license for terms.
0003
0004 #ifndef tools_qrot
0005 #define tools_qrot
0006
0007 // rotation done with quaternion.
0008
0009 namespace tools {
0010
0011 template <class VEC3,class VEC4>
0012 class qrot {
0013 protected:
0014 typedef typename VEC4::elem_t T; //we assume = T3
0015 public:
0016 qrot()
0017 :m_quat(0,0,0,1) //zero rotation around the positive Z axis.
0018 {}
0019 qrot(const VEC3& a_axis,T a_radians,T(*a_sin)(T),T(*a_cos)(T)){
0020 if(!set_value(a_axis,a_radians,a_sin,a_cos)) {} //FIXME : throw
0021 }
0022 qrot(const VEC3& a_from,const VEC3& a_to,T(*a_sqrt)(T),T(*a_fabs)(T)){set_value(a_from,a_to,a_sqrt,a_fabs);}
0023 virtual ~qrot(){}
0024 public:
0025 qrot(const qrot& a_from)
0026 :m_quat(a_from.m_quat)
0027 {}
0028 qrot& operator=(const qrot& a_from){
0029 m_quat = a_from.m_quat;
0030 return *this;
0031 }
0032 protected:
0033 qrot(T a_q0,T a_q1,T a_q2,T a_q3)
0034 :m_quat(a_q0,a_q1,a_q2,a_q3)
0035 {
0036 if(!m_quat.normalize()) {} //FIXME throw
0037 }
0038
0039 public:
0040 qrot& operator*=(const qrot& a_q) {
0041 //Multiplies the quaternions.
0042 //Note that order is important when combining quaternions with the
0043 //multiplication operator.
0044 // Formula from <http://www.lboro.ac.uk/departments/ma/gallery/quat/>
0045
0046 T tx = m_quat.v0();
0047 T ty = m_quat.v1();
0048 T tz = m_quat.v2();
0049 T tw = m_quat.v3();
0050
0051 T qx = a_q.m_quat.v0();
0052 T qy = a_q.m_quat.v1();
0053 T qz = a_q.m_quat.v2();
0054 T qw = a_q.m_quat.v3();
0055
0056 m_quat.set_value(qw*tx + qx*tw + qy*tz - qz*ty,
0057 qw*ty - qx*tz + qy*tw + qz*tx,
0058 qw*tz + qx*ty - qy*tx + qz*tw,
0059 qw*tw - qx*tx - qy*ty - qz*tz);
0060 m_quat.normalize();
0061 return *this;
0062 }
0063
0064 bool operator==(const qrot& a_r) const {
0065 return m_quat.equal(a_r.m_quat);
0066 }
0067 bool operator!=(const qrot& a_r) const {
0068 return !operator==(a_r);
0069 }
0070 qrot operator*(const qrot& a_r) const {
0071 qrot tmp(*this);
0072 tmp *= a_r;
0073 return tmp;
0074 }
0075
0076 bool invert(){
0077 T length = m_quat.length();
0078 if(length==T()) return false;
0079
0080 // Optimize by doing 1 div and 4 muls instead of 4 divs.
0081 T inv = one() / length;
0082
0083 m_quat.set_value(-m_quat.v0() * inv,
0084 -m_quat.v1() * inv,
0085 -m_quat.v2() * inv,
0086 m_quat.v3() * inv);
0087
0088 return true;
0089 }
0090
0091 bool inverse(qrot& a_r) const {
0092 //Non-destructively inverses the rotation and returns the result.
0093 T length = m_quat.length();
0094 if(length==T()) return false;
0095
0096 // Optimize by doing 1 div and 4 muls instead of 4 divs.
0097 T inv = one() / length;
0098
0099 a_r.m_quat.set_value(-m_quat.v0() * inv,
0100 -m_quat.v1() * inv,
0101 -m_quat.v2() * inv,
0102 m_quat.v3() * inv);
0103
0104 return true;
0105 }
0106
0107
0108 bool set_value(const VEC3& a_axis,T a_radians,T(*a_sin)(T),T(*a_cos)(T)) {
0109 // Reset rotation with the given axis-of-rotation and rotation angle.
0110 // Make sure axis is not the null vector when calling this method.
0111 // From <http://www.automation.hut.fi/~jaro/thesis/hyper/node9.html>.
0112 if(a_axis.length()==T()) return false;
0113 m_quat.v3(a_cos(a_radians/2));
0114 T sineval = a_sin(a_radians/2);
0115 VEC3 a = a_axis;
0116 a.normalize();
0117 m_quat.v0(a.v0() * sineval);
0118 m_quat.v1(a.v1() * sineval);
0119 m_quat.v2(a.v2() * sineval);
0120 return true;
0121 }
0122
0123 bool set_value(const VEC3& a_from,const VEC3& a_to,T(*a_sqrt)(T),T(*a_fabs)(T)) {
0124 // code taken from coin3d/SbRotation.
0125 //NOTE : coin3d/SbMatrix logic is transposed relative to us.
0126
0127 VEC3 from(a_from);
0128 if(from.normalize()==T()) return false;
0129 VEC3 to(a_to);
0130 if(to.normalize()==T()) return false;
0131
0132 T dot = from.dot(to);
0133 VEC3 crossvec;from.cross(to,crossvec);
0134 T crosslen = crossvec.normalize();
0135
0136 if(crosslen == T()) { // Parallel vectors
0137 // Check if they are pointing in the same direction.
0138 if (dot > T()) {
0139 m_quat.set_value(0,0,0,1);
0140 } else {
0141 // Ok, so they are parallel and pointing in the opposite direction
0142 // of each other.
0143 // Try crossing with x axis.
0144 VEC3 t;from.cross(VEC3(1,0,0),t);
0145 // If not ok, cross with y axis.
0146 if(t.normalize() == T()) {
0147 from.cross(VEC3(0,1,0),t);
0148 t.normalize();
0149 }
0150 m_quat.set_value(t[0],t[1],t[2],0);
0151 }
0152 } else { // Vectors are not parallel
0153 // The fabs() wrapping is to avoid problems when `dot' "overflows"
0154 // a tiny wee bit, which can lead to sqrt() returning NaN.
0155 crossvec *= (T)a_sqrt(half() * a_fabs(one() - dot));
0156 // The fabs() wrapping is to avoid problems when `dot' "underflows"
0157 // a tiny wee bit, which can lead to sqrt() returning NaN.
0158 m_quat.set_value(crossvec[0], crossvec[1], crossvec[2],(T)a_sqrt(half()*a_fabs(one()+dot)));
0159 }
0160
0161 return true;
0162 }
0163
0164
0165 bool value(VEC3& a_axis,T& a_radians,T(*a_sin)(T),T(*a_acos)(T)) const { //WARNING a_acos and NOT a_cos
0166 //WARNING : can fail.
0167 if( (m_quat.v3()<minus_one()) || (m_quat.v3()> one()) ) {
0168 a_axis.set_value(0,0,1);
0169 a_radians = 0;
0170 return false;
0171 }
0172
0173 a_radians = a_acos(m_quat.v3()) * 2; //in [0,2*pi]
0174 T sineval = a_sin(a_radians/2);
0175
0176 if(sineval==T()) { //a_radian = 0 or 2*pi.
0177 a_axis.set_value(0,0,1);
0178 a_radians = 0;
0179 return false;
0180 }
0181 a_axis.set_value(m_quat.v0()/sineval,
0182 m_quat.v1()/sineval,
0183 m_quat.v2()/sineval);
0184 return true;
0185 }
0186
0187 template <class MAT4>
0188 void set_value(const MAT4& a_m,T(*a_sqrt)(T)) {
0189 // See tests/qrot.icc.
0190 // code taken from coin3d/SbRotation.
0191
0192 //Set the rotation from the components of the given matrix.
0193
0194 T scalerow = a_m.v00() + a_m.v11() + a_m.v22();
0195 if (scalerow > T()) {
0196 T _s = a_sqrt(scalerow + a_m.v33());
0197 m_quat.v3(_s * half());
0198 _s = half() / _s;
0199
0200 m_quat.v0((a_m.v21() - a_m.v12()) * _s);
0201 m_quat.v1((a_m.v02() - a_m.v20()) * _s);
0202 m_quat.v2((a_m.v10() - a_m.v01()) * _s);
0203 } else {
0204 unsigned int i = 0;
0205 if (a_m.v11() > a_m.v00()) i = 1;
0206 if (a_m.v22() > a_m.value(i,i)) i = 2;
0207
0208 unsigned int j = (i+1)%3;
0209 unsigned int k = (j+1)%3;
0210
0211 T _s = a_sqrt((a_m.value(i,i) - (a_m.value(j,j) + a_m.value(k,k))) + a_m.v33());
0212
0213 m_quat.set_value(i,_s * half());
0214 _s = half() / _s;
0215
0216 m_quat.v3((a_m.value(k,j) - a_m.value(j,k)) * _s);
0217 m_quat.set_value(j,(a_m.value(j,i) + a_m.value(i,j)) * _s);
0218 m_quat.set_value(k,(a_m.value(k,i) + a_m.value(i,k)) * _s);
0219 }
0220
0221 if (a_m.v33()!=one()) {
0222 m_quat.multiply(one()/a_sqrt(a_m.v33()));
0223 }
0224 }
0225
0226 template <class MAT4>
0227 void value(MAT4& a_m) const {
0228 //Return this rotation in the form of a matrix.
0229 //NOTE : in coin3d/SbRotation, it looks as if "w <-> -w", but coin3d/SbMatrix logic is transposed relative to us.
0230
0231 const T x = m_quat.v0();
0232 const T y = m_quat.v1();
0233 const T z = m_quat.v2();
0234 const T w = m_quat.v3();
0235 // q = w + x * i + y * j + z * k
0236
0237 // first row :
0238 a_m.v00(w*w + x*x - y*y - z*z);
0239 a_m.v01(2*x*y - 2*w*z);
0240 a_m.v02(2*x*z + 2*w*y);
0241 a_m.v03(0);
0242
0243 // second row :
0244 a_m.v10(2*x*y + 2*w*z);
0245 a_m.v11(w*w - x*x + y*y - z*z);
0246 a_m.v12(2*y*z - 2*w*x);
0247 a_m.v13(0);
0248
0249 // third row :
0250 a_m.v20(2*x*z - 2*w*y);
0251 a_m.v21(2*y*z + 2*w*x);
0252 a_m.v22(w*w - x*x - y*y + z*z);
0253 a_m.v23(0);
0254
0255 // fourth row :
0256 a_m.v30(0);
0257 a_m.v31(0);
0258 a_m.v32(0);
0259 a_m.v33(w*w + x*x + y*y + z*z);
0260 }
0261
0262 template <class MAT3>
0263 T value_3(MAT3& a_m) const {
0264 //Return this rotation in the form of a 3D matrix.
0265 //NOTE : in coin3d/SbRotation, it looks as if "w <-> -w", but coin3d/SbMatrix logic is transposed relative to us.
0266
0267 const T x = m_quat.v0();
0268 const T y = m_quat.v1();
0269 const T z = m_quat.v2();
0270 const T w = m_quat.v3();
0271 // q = w + x * i + y * j + z * k
0272
0273 // first row :
0274 a_m.v00(w*w + x*x - y*y - z*z);
0275 a_m.v01(2*x*y - 2*w*z);
0276 a_m.v02(2*x*z + 2*w*y);
0277
0278 // second row :
0279 a_m.v10(2*x*y + 2*w*z);
0280 a_m.v11(w*w - x*x + y*y - z*z);
0281 a_m.v12(2*y*z - 2*w*x);
0282
0283 // third row :
0284 a_m.v20(2*x*z - 2*w*y);
0285 a_m.v21(2*y*z + 2*w*x);
0286 a_m.v22(w*w - x*x - y*y + z*z);
0287
0288 return w*w + x*x + y*y + z*z; //should be 1.
0289 }
0290
0291 void mul_vec(const VEC3& a_in,VEC3& a_out) const {
0292 const T x = m_quat.v0();
0293 const T y = m_quat.v1();
0294 const T z = m_quat.v2();
0295 const T w = m_quat.v3();
0296
0297 // first row :
0298 T v0 = (w*w + x*x - y*y - z*z) * a_in.v0()
0299 + (2*x*y - 2*w*z) * a_in.v1()
0300 + (2*x*z + 2*w*y) * a_in.v2();
0301
0302 T v1 = (2*x*y + 2*w*z) * a_in.v0()
0303 +(w*w - x*x + y*y - z*z) * a_in.v1()
0304 + (2*y*z - 2*w*x) * a_in.v2();
0305
0306 T v2 = (2*x*z - 2*w*y) * a_in.v0()
0307 + (2*y*z + 2*w*x) * a_in.v1()
0308 +(w*w - x*x - y*y + z*z) * a_in.v2();
0309
0310 a_out.set_value(v0,v1,v2);
0311 }
0312
0313 void mul_vec(VEC3& a_v) const {
0314 const T x = m_quat.v0();
0315 const T y = m_quat.v1();
0316 const T z = m_quat.v2();
0317 const T w = m_quat.v3();
0318
0319 // first row :
0320 T v0 = (w*w + x*x - y*y - z*z) * a_v.v0()
0321 + (2*x*y - 2*w*z) * a_v.v1()
0322 + (2*x*z + 2*w*y) * a_v.v2();
0323
0324 T v1 = (2*x*y + 2*w*z) * a_v.v0()
0325 +(w*w - x*x + y*y - z*z) * a_v.v1()
0326 + (2*y*z - 2*w*x) * a_v.v2();
0327
0328 T v2 = (2*x*z - 2*w*y) * a_v.v0()
0329 + (2*y*z + 2*w*x) * a_v.v1()
0330 +(w*w - x*x - y*y + z*z) * a_v.v2();
0331
0332 a_v.set_value(v0,v1,v2);
0333 }
0334
0335 void mul_3(T& a_x,T& a_y,T& a_z) const {
0336 const T x = m_quat.v0();
0337 const T y = m_quat.v1();
0338 const T z = m_quat.v2();
0339 const T w = m_quat.v3();
0340
0341 // first row :
0342 T v0 = (w*w + x*x - y*y - z*z) * a_x
0343 + (2*x*y - 2*w*z) * a_y
0344 + (2*x*z + 2*w*y) * a_z;
0345
0346 T v1 = (2*x*y + 2*w*z) * a_x
0347 +(w*w - x*x + y*y - z*z) * a_y
0348 + (2*y*z - 2*w*x) * a_z;
0349
0350 T v2 = (2*x*z - 2*w*y) * a_x
0351 + (2*y*z + 2*w*x) * a_y
0352 +(w*w - x*x - y*y + z*z) * a_z;
0353
0354 a_x = v0;
0355 a_y = v1;
0356 a_z = v2;
0357 }
0358
0359 public: //for io::streamer
0360 const VEC4& quat() const {return m_quat;}
0361 VEC4& quat() {return m_quat;}
0362
0363 protected:
0364 static T one() {return T(1);}
0365 static T minus_one() {return T(-1);}
0366 static T half() {return T(0.5);}
0367
0368 protected:
0369 VEC4 m_quat;
0370 };
0371
0372 }
0373
0374 #endif
