home *** CD-ROM | disk | FTP | other *** search
/ Chip 2001 February / Chip_2001-02_cd1.bin / bonus / demos / CS / exp / SOURCES / GLENGINE / quaternion.h < prev    next >
C/C++ Source or Header  |  2000-08-07  |  12KB  |  440 lines
  1. // Template and inlines implementing quaternion number support for C++
  2. // Written by Boris Burger (flashp@users.sourceforge.net)
  3. // updated 14 Jul 2000
  4.  
  5. #ifndef __QUATERNION__
  6. #define __QUATERNION__
  7.  
  8. #ifdef __GNUG__
  9. #pragma interface
  10. #endif
  11.  
  12. #if !defined (__GNUG__) && !defined (__attribute__)
  13. #define __attribute__(foo) /* Ignore.  */
  14. #endif
  15.  
  16. #include <iostream>
  17. #include <cmath>
  18.  
  19. extern "C++" {
  20.  
  21. template <class T> class quaternion;
  22. template <class T> quaternion<T>&
  23.   __doapl (quaternion<T>* ths, const quaternion<T>& r);
  24. template <class T> quaternion<T>&
  25.   __doami (quaternion<T>* ths, const quaternion<T>& r);
  26. template <class T> quaternion<T>&
  27.   __doaml (quaternion<T>* ths, const quaternion<T>& r);
  28.  
  29. template <class T> class quaternion
  30. {
  31. private:
  32.  
  33.   mutable T a0, a1, a2, a3;
  34.  
  35. public:
  36.  
  37.   // constructor
  38.  
  39.   quaternion(T x0=0, T x1=0, T x2=0, T x3=0):
  40.              a0 (x0), a1 (x1), a2 (x2), a3 (x3) { }
  41.  
  42.   // function prototypes
  43.  
  44.   quaternion& operator += (const quaternion&);
  45.   quaternion& operator -= (const quaternion&);
  46.   quaternion& operator *= (const quaternion&);
  47.  
  48.   friend quaternion& __doapl<> (quaternion *, const quaternion&);
  49.   friend quaternion& __doami<> (quaternion *, const quaternion&);
  50.   friend quaternion& __doaml<> (quaternion *, const quaternion&);
  51.  
  52.   T re () const { return a0; }
  53.   T im1 () const { return a1; }
  54.   T im2 () const { return a2; }
  55.   T im3 () const { return a3; }
  56. };
  57.  
  58. // specializations
  59.  
  60. template class quaternion<float>;
  61. template class quaternion<double>;
  62. template class quaternion<long double>;
  63.  
  64. // aliases
  65.  
  66. typedef quaternion<float> float_quaternion;
  67. typedef quaternion<double> double_quaternion;
  68. typedef quaternion<long double> long_double_quaternion;
  69.  
  70. // function definitions
  71.  
  72. template <class T>
  73. ostream& operator<<(ostream& s, const quaternion<T>& q)
  74.   {
  75.     return(s<<"("<<q.re()<<","<<q.im1()<<","<<q.im2()<<","<<q.im3()<<")");
  76.   }
  77.  
  78. template <class T>
  79. inline quaternion<T>&
  80. __doapl (quaternion<T>* ths, const quaternion<T>& r)
  81. {
  82.   ths->a0 += r.a0;
  83.   ths->a1 += r.a1;
  84.   ths->a2 += r.a2;
  85.   ths->a3 += r.a3;
  86.   return *ths;
  87. }
  88. template <class T>
  89. inline quaternion<T>&
  90. quaternion<T>::operator += (const quaternion<T>& r)
  91. {
  92.   return __doapl (this, r);
  93. }
  94.  
  95. template <class T>
  96. inline quaternion<T>&
  97. __doami (quaternion<T>* ths, const quaternion<T>& r)
  98. {
  99.   ths->a0 -= r.a0;
  100.   ths->a1 -= r.a1;
  101.   ths->a2 -= r.a2;
  102.   ths->a3 -= r.a3;
  103.   return *ths;
  104. }
  105. template <class T>
  106. inline quaternion<T>&
  107. quaternion<T>::operator -= (const quaternion<T>& r)
  108. {
  109.   return __doami (this, r);
  110. }
  111.  
  112. template <class T>
  113. inline quaternion<T>&
  114. __doaml (quaternion<T>* ths, const quaternion<T>& r)
  115. {
  116.   T aa0 = ths->a0*r.a0 - ths->a1*r.a1 - ths->a2*r.a2 - ths->a3*r.a3;
  117.   T aa1 = ths->a0*r.a1 + ths->a1*r.a0 + ths->a2*r.a3 - ths->a3*r.a2;
  118.   T aa2 = ths->a0*r.a2 + ths->a2*r.a0 + ths->a3*r.a1 - ths->a1*r.a3;
  119.   T aa3 = ths->a0*r.a3 + ths->a3*r.a0 + ths->a1*r.a2 - ths->a2*r.a1;
  120.   ths->a0 = aa0;
  121.   ths->a1 = aa1;
  122.   ths->a2 = aa2;
  123.   ths->a3 = aa3;
  124.   return *ths;
  125. }
  126.  
  127. template <class T>
  128. inline quaternion<T>&
  129. quaternion<T>::operator *= (const quaternion<T>& r)
  130. {
  131.   return __doaml (this, r);
  132. }
  133.  
  134. template <class T> inline T
  135. re (const quaternion<T>& x) __attribute__ ((const));
  136.  
  137. template <class T> inline T
  138. re (const quaternion<T>& x)
  139. {
  140.   return x.re ();
  141. }
  142.  
  143. template <class T> inline T
  144. im1 (const quaternion<T>& x) __attribute__ ((const));
  145.  
  146. template <class T> inline T
  147. im1 (const quaternion<T>& x)
  148. {
  149.   return x.im1 ();
  150. }
  151.  
  152. template <class T> inline T
  153. im2 (const quaternion<T>& x) __attribute__ ((const));
  154.  
  155. template <class T> inline T
  156. im2 (const quaternion<T>& x)
  157. {
  158.   return x.im2 ();
  159. }
  160.  
  161. template <class T> inline T
  162. im3 (const quaternion<T>& x) __attribute__ ((const));
  163.  
  164. template <class T> inline T
  165. im3 (const quaternion<T>& x)
  166. {
  167.   return x.im3 ();
  168. }
  169.  
  170.  
  171. template <class T> inline quaternion<T>
  172. operator + (const quaternion<T>& x, const quaternion<T>& y) __attribute__ ((const));
  173.  
  174. template <class T> inline quaternion<T>
  175. operator + (const quaternion<T>& x, const quaternion<T>& y)
  176. {
  177.   return quaternion<T> ( re(x)+re(y), im1(x)+im1(y),
  178.                             im2(x)+im2(y), im3(x)+im3(y));
  179. }
  180.  
  181. template <class T> inline quaternion<T>
  182. operator + (const quaternion<T>& x, T y) __attribute__ ((const));
  183.  
  184. template <class T> inline quaternion<T>
  185. operator + (const quaternion<T>& x, T y)
  186. {
  187.   return quaternion<T> (re(x)+y, im1(x), im2(x), im3(x));
  188. }
  189.  
  190. template <class T> inline quaternion<T>
  191. operator + (T x, const quaternion<T>& y) __attribute__ ((const));
  192.  
  193. template <class T> inline quaternion<T>
  194. operator + (T x, const quaternion<T>& y)
  195. {
  196.   return quaternion<T> (x + re(y), im1(y), im2(y), im3(y));
  197. }
  198.  
  199. template <class T> inline quaternion<T>
  200. operator - (const quaternion<T>& x, const quaternion<T>& y) __attribute__ ((const));
  201.  
  202. template <class T> inline quaternion<T>
  203. operator - (const quaternion<T>& x, const quaternion<T>& y)
  204. {
  205.   return quaternion<T> ( re(x)-re(y), im1(x)-im1(y),
  206.                             im2(x)-im2(y), im3(x)-im3(y));
  207. }
  208.  
  209. template <class T> inline quaternion<T>
  210. operator - (const quaternion<T>& x, T y) __attribute__ ((const));
  211.  
  212. template <class T> inline quaternion<T>
  213. operator - (const quaternion<T>& x, T y)
  214. {
  215.   return quaternion<T> (re(x) - y, im1 (x), im2 (x), im3 (x));
  216. }
  217.  
  218. template <class T> inline quaternion<T>
  219. operator - (T x, const quaternion<T>& y) __attribute__ ((const));
  220.  
  221. template <class T> inline quaternion<T>
  222. operator - (T x, const quaternion<T>& y)
  223. {
  224.   return quaternion<T> (x - re (y), - im1 (y), - im2 (y), - im3 (y));
  225. }
  226.  
  227. template <class T> inline quaternion<T>
  228. operator * (const quaternion<T>& x, const quaternion<T>& y) __attribute__ ((const));
  229.  
  230. template <class T> inline quaternion<T>
  231. operator * (const quaternion<T>& x, const quaternion<T>& y)
  232. {
  233.   return quaternion<T>
  234.              (re(x)*re(y) - im1(x)*im1(y) - im2(x)*im2(y) - im3(x)*im3(y), 
  235.               re(x)*im1(y) + im1(x)*re(y) + im2(x)*im3(y) - im3(x)*im2(y), 
  236.               re(x)*im2(y) + im2(x)*re(y) + im3(x)*im1(y) - im1(x)*im3(y), 
  237.               re(x)*im3(y) + im3(x)*re(y) + im1(x)*im2(y) - im2(x)*im1(y) );
  238. }
  239.  
  240. template <class T> inline quaternion<T>
  241. operator * (const quaternion<T>& x, T y) __attribute__ ((const));
  242.  
  243. template <class T> inline quaternion<T>
  244. operator * (const quaternion<T>& x, T y)
  245. {
  246.   return quaternion<T> (re (x) * y, im1 (x) * y, im2 (x) * y, im3 (x) * y);
  247. }
  248.  
  249. template <class T> inline quaternion<T>
  250. operator * (T x, const quaternion<T>& y) __attribute__ ((const));
  251.  
  252. template <class T> inline quaternion<T>
  253. operator * (T x, const quaternion<T>& y)
  254. {
  255.   return quaternion<T> (x * re (y), x * im1 (y), x * im2 (y), x * im3 (y));
  256. }
  257.  
  258. template <class T> quaternion<T>
  259. operator / (const quaternion<T>& x, T y) __attribute__ ((const));
  260.  
  261. template <class T> quaternion<T>
  262. operator / (const quaternion<T>& x, T y)
  263. {
  264.   return quaternion<T> (re (x) / y, im1 (x) / y, im2 (x) / y, im3 (x) / y);
  265. }
  266.  
  267. template <class T> inline quaternion<T>
  268. operator + (const quaternion<T>& x) __attribute__ ((const));
  269.  
  270. template <class T> inline quaternion<T>
  271. operator + (const quaternion<T>& x)
  272. {
  273.   return x;
  274. }
  275.  
  276. template <class T> inline quaternion<T>
  277. operator - (const quaternion<T>& x) __attribute__ ((const));
  278.  
  279. template <class T> inline quaternion<T>
  280. operator - (const quaternion<T>& x)
  281. {
  282.   return quaternion<T> (-re (x), -im1 (x), -im2 (x), -im3 (x));
  283. }
  284.  
  285.  
  286. template <class T> inline bool
  287. operator == (const quaternion<T>& x, const quaternion<T>& y) __attribute__ ((const));
  288.  
  289. template <class T> inline bool
  290. operator == (const quaternion<T>& x, const quaternion<T>& y)
  291. {
  292.   return re(x)==re(y) && im1(x)==im1(y) && im2(x)==im2(y) && im3(x)==im3(y);
  293. }
  294.  
  295. template <class T> inline bool
  296. operator == (const quaternion<T>& x, T y) __attribute__ ((const));
  297.  
  298. template <class T> inline bool
  299. operator == (const quaternion<T>& x, T y)
  300. {
  301.   return re(x)==y && im1(x) == 0 && im2(x) == 0 && im3(x) == 0;
  302. }
  303.  
  304. template <class T> inline bool
  305. operator == (T x, const quaternion<T>& y) __attribute__ ((const));
  306.  
  307. template <class T> inline bool
  308. operator == (T x, const quaternion<T>& y)
  309. {
  310.   return x==re(y) && im1(y)==0 && im2(y)==0 && im3(y)==0;
  311. }
  312.  
  313. template <class T> inline bool
  314. operator != (const quaternion<T>& x, const quaternion<T>& y) __attribute__ ((const));
  315.  
  316. template <class T> inline bool
  317. operator != (const quaternion<T>& x, const quaternion<T>& y)
  318. {
  319.   return re(x)!=re(y) || im1(x)!=im1(y) || im2(x)!=im2(y) || im3(x)!=im3(y);
  320. }
  321.  
  322. template <class T> inline bool
  323. operator != (const quaternion<T>& x, T y) __attribute__ ((const));
  324.  
  325. template <class T> inline bool
  326. operator != (const quaternion<T>& x, T y)
  327. {
  328.   return re(x)!=y || im1(x)!=0 || im2(x)!=0 || im3(x)!=0;
  329. }
  330.  
  331. template <class T> inline bool
  332. operator != (T x, const quaternion<T>& y) __attribute__ ((const));
  333.  
  334. template <class T> inline bool
  335. operator != (T x, const quaternion<T>& y)
  336. {
  337.   return x!=re(y) || im1(y)!=0 || im2(y)!=0 || im3(y)!=0;
  338. }
  339.  
  340. template <class T> inline T
  341. abs (const quaternion<T>& x) __attribute__ ((const));
  342.  
  343. template <class T> inline T
  344. abs (const quaternion<T>& x)
  345. {
  346.   return sqrt(re(x)*re(x) + im1(x)*im1(x) + im2(x)*im2(x) + im3(x)*im3(x));
  347. }
  348.  
  349. template <class T> inline T
  350. arg (const quaternion<T>& x) __attribute__ ((const));
  351.  
  352. template <class T> inline T
  353. arg (const quaternion<T>& x)
  354. {
  355.   return atan2(sqrt(im1(x)*im1(x) + im2(x)*im2(x) + im3(x)*im3(x)), re(x));
  356. }
  357.  
  358. template <class T> inline quaternion<T>
  359. conj (const quaternion<T>& x)  __attribute__ ((const));
  360.  
  361. template <class T> inline quaternion<T>
  362. conj (const quaternion<T>& x) 
  363. {
  364.   return quaternion<T> (re(x), -im1(x), -im2(x), -im3(x));
  365. }
  366.  
  367. template <class T> inline T
  368. norm (const quaternion<T>& x) __attribute__ ((const));
  369.  
  370. template <class T> inline T
  371. norm (const quaternion<T>& x)
  372. {
  373.   return re(x)*re(x) + im1(x)*im1(x) + im2(x)*im2(x) + im3(x)*im3(x);
  374. }
  375.  
  376. template <class T> inline T
  377. dot (const quaternion<T>& x, const quaternion<T>& y) __attribute__ ((const));
  378.  
  379. template <class T> inline T
  380. dot (const quaternion<T>& x, const quaternion<T>& y)
  381. {
  382.   return re(x)*re(y) + im1(x)*im1(y) + im2(x)*im2(y) + im3(x)*im3(y);
  383. }
  384.  
  385. template <class T> inline quaternion<T>
  386. slerp (T t, const quaternion<T>& x, const quaternion<T>& y) __attribute__ ((const));
  387.  
  388. template <class T> inline quaternion<T>
  389. slerp (T t, const quaternion<T>& x, const quaternion<T>& y)
  390. {
  391.   double a = acos(dot(x, y));
  392.   double sa = sin(a);
  393.   if(sa>=(1e-06))
  394.     return x*(T)(sin((1.0-t)*a)/sin(a)) + y*(T)(sin(t*a)/sin(a));
  395.   return x*(T)(1.0-t) + y*(T)(t);
  396. }
  397.  
  398.  
  399.  
  400. template <class T> inline quaternion<T>
  401. exp (const quaternion<T>& x) __attribute__ ((const));
  402.  
  403. template <class T> inline quaternion<T>
  404. exp (const quaternion<T>& x)
  405. {
  406.   T angle = sqrt(im1(x)*im1(x) + im2(x)*im2(x) + im3(x)*im3(x));
  407.   T cs = cos(angle);
  408.   T sn = sin(angle);
  409.  
  410.   if ( fabs(sn) >= (1e-06) ) {
  411.     T coeff = sn/angle;
  412.     return quaternion<T> (cs, coeff*im1(x), coeff*im2(x), coeff*im3(x));
  413.     }
  414.   return quaternion<T> (cs, im1(x), im2(x), im3(x));
  415. }
  416.  
  417. template <class T> inline quaternion<T>
  418. log (const quaternion<T>& x) __attribute__ ((const));
  419.  
  420. template <class T> inline quaternion<T>
  421. log (const quaternion<T>& x)
  422. {
  423.   if(fabs(re(x))<1)
  424.   {
  425.     T angle = acos(re(x));
  426.     T sn = sin(angle);
  427.     if(fabs(sn)>=(1e-06))
  428.     {
  429.       T coeff = angle/sn;
  430.       return quaternion<T>(0, coeff*im1(x), coeff*im2(x), coeff*im3(x));
  431.     }
  432.   }
  433.   return quaternion<T> (0, im1(x), im2(x), im3(x));
  434. }
  435.  
  436. } // extern "C++"
  437.  
  438. #endif
  439.  
  440.