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