libs/math/vector.h

   1 /*
   2 Copyright (C) 2001-2006, William Joseph.
   3 All Rights Reserved.
   4
   5 This file is part of GtkRadiant.
   6
   7 GtkRadiant is free software; you can redistribute it and/or modify
   8 it under the terms of the GNU General Public License as published by
   9 the Free Software Foundation; either version 2 of the License, or
  10 (at your option) any later version.
  11
  12 GtkRadiant is distributed in the hope that it will be useful,
  13 but WITHOUT ANY WARRANTY; without even the implied warranty of
  14 MERCHANTABILITY or FITNESS FOR A PARTICULAR PURPOSE.  See the
  15 GNU General Public License for more details.
  16
  17 You should have received a copy of the GNU General Public License
  18 along with GtkRadiant; if not, write to the Free Software
  19 Foundation, Inc., 51 Franklin St, Fifth Floor, Boston, MA  02110-1301  USA
  20 */
  21
  22 #if !defined(INCLUDED_MATH_VECTOR_H)
  23 #define INCLUDED_MATH_VECTOR_H
  24
  25 /// \file
  26 /// \brief Vector data types and related operations.
  27
  28 #if 0
  29
  30 #define lrint(dbl)              ((int)((dbl) + 0.5))
  31 #define lrintf(flt)             ((int)((flt) + 0.5))
  32
  33 #endif
  34
  35 #if defined (_MSC_VER)
  36
  37 inline int lrint (double flt)
  38 {
  39   int i;
  40
  41         _asm
  42         {
  43     fld flt
  44                 fistp i
  45   };
  46
  47         return i;
  48 }
  49
  50 #else // lrint is part of ISO C99
  51
  52 #define _ISOC9X_SOURCE  1
  53 #define _ISOC99_SOURCE  1
  54
  55 #define __USE_ISOC9X    1
  56 #define __USE_ISOC99    1
  57
  58 #endif
  59
  60 #include <cmath>
  61 #include <cstddef>
  62 #include <float.h>
  63 #include <algorithm>
  64
  65 //#include "debugging/debugging.h"
  66
  67 /// \brief Returns true if \p self is equal to other \p other within \p epsilon.
  68 template<typename Element, typename OtherElement>
  69 inline bool float_equal_epsilon(const Element& self, const OtherElement& other, const Element& epsilon)
  70 {
  71   return fabs(other - self) < epsilon;
  72 }
  73
  74 /// \brief Returns the value midway between \p self and \p other.
  75 template<typename Element>
  76 inline Element float_mid(const Element& self, const Element& other)
  77 {
  78   return Element((self + other) * 0.5);
  79 }
  80
  81 /// \brief Returns \p f rounded to the nearest integer. Note that this is not the same behaviour as casting from float to int.
  82 template<typename Element>
  83 inline int float_to_integer(const Element& f)
  84 {
  85   return lrint(f);
  86 }
  87
  88 /// \brief Returns \p f rounded to the nearest multiple of \p snap.
  89 template<typename Element, typename OtherElement>
  90 inline Element float_snapped(const Element& f, const OtherElement& snap)
  91 {
  92   return Element(float_to_integer(f / snap) * snap);
  93 }
  94
  95 /// \brief Returns true if \p f has no decimal fraction part.
  96 template<typename Element>
  97 inline bool float_is_integer(const Element& f)
  98 {
  99   return f == Element(float_to_integer(f));
 100 }
 101
 102 /// \brief Returns \p self modulated by the range [0, \p modulus)
 103 /// \p self must be in the range [\p -modulus, \p modulus)
 104 template<typename Element, typename ModulusElement>
 105 inline Element float_mod_range(const Element& self, const ModulusElement& modulus)
 106 {
 107   return Element((self < 0.0) ? self + modulus : self);
 108 }
 109
 110 /// \brief Returns \p self modulated by the range [0, \p modulus)
 111 template<typename Element, typename ModulusElement>
 112 inline Element float_mod(const Element& self, const ModulusElement& modulus)
 113 {
 114   return float_mod_range(Element(fmod(static_cast<double>(self), static_cast<double>(modulus))), modulus);
 115 }
 116
 117
 118 template<typename Element>
 119 class BasicVector2
 120 {
 121   Element m_elements[2];
 122 public:
 123   BasicVector2()
 124   {
 125   }
 126   BasicVector2(const Element& x_, const Element& y_)
 127   {
 128     x() = x_;
 129     y() = y_;
 130   }
 131
 132   Element& x()
 133   {
 134     return m_elements[0];
 135   }
 136   const Element& x() const
 137   {
 138     return m_elements[0];
 139   }
 140   Element& y()
 141   {
 142     return m_elements[1];
 143   }
 144   const Element& y() const
 145   {
 146     return m_elements[1];
 147   }
 148
 149   const Element& operator[](std::size_t i) const
 150   {
 151     return m_elements[i];
 152   }
 153   Element& operator[](std::size_t i)
 154   {
 155     return m_elements[i];
 156   }
 157 };
 158
 159
 160 template<typename Element, typename OtherElement>
 161 inline BasicVector2<Element> vector2_added(const BasicVector2<Element>& self, const BasicVector2<OtherElement>& other)
 162 {
 163   return BasicVector2<Element>(
 164     Element(self.x() + other.x()),
 165     Element(self.y() + other.y())
 166   );
 167 }
 168 template<typename Element, typename OtherElement>
 169 inline BasicVector2<Element> operator+(const BasicVector2<Element>& self, const BasicVector2<OtherElement>& other)
 170 {
 171   return vector2_added(self, other);
 172 }
 173 template<typename Element, typename OtherElement>
 174 inline void vector2_add(BasicVector2<Element>& self, const BasicVector2<OtherElement>& other)
 175 {
 176   self.x() += Element(other.x());
 177   self.y() += Element(other.y());
 178 }
 179 template<typename Element, typename OtherElement>
 180 inline void operator+=(BasicVector2<Element>& self, const BasicVector2<OtherElement>& other)
 181 {
 182   vector2_add(self, other);
 183 }
 184
 185
 186 template<typename Element, typename OtherElement>
 187 inline BasicVector2<Element> vector2_subtracted(const BasicVector2<Element>& self, const BasicVector2<OtherElement>& other)
 188 {
 189   return BasicVector2<Element>(
 190     Element(self.x() - other.x()),
 191     Element(self.y() - other.y())
 192   );
 193 }
 194 template<typename Element, typename OtherElement>
 195 inline BasicVector2<Element> operator-(const BasicVector2<Element>& self, const BasicVector2<OtherElement>& other)
 196 {
 197   return vector2_subtracted(self, other);
 198 }
 199 template<typename Element, typename OtherElement>
 200 inline void vector2_subtract(BasicVector2<Element>& self, const BasicVector2<OtherElement>& other)
 201 {
 202   self.x() -= Element(other.x());
 203   self.y() -= lement(other.y());
 204 }
 205 template<typename Element, typename OtherElement>
 206 inline void operator-=(BasicVector2<Element>& self, const BasicVector2<OtherElement>& other)
 207 {
 208   vector2_subtract(self, other);
 209 }
 210
 211
 212 template<typename Element, typename OtherElement>
 213 inline BasicVector2<Element> vector2_scaled(const BasicVector2<Element>& self, OtherElement other)
 214 {
 215   return BasicVector2<Element>(
 216     Element(self.x() * other),
 217     Element(self.y() * other)
 218   );
 219 }
 220 template<typename Element, typename OtherElement>
 221 inline BasicVector2<Element> operator*(const BasicVector2<Element>& self, OtherElement other)
 222 {
 223   return vector2_scaled(self, other);
 224 }
 225 template<typename Element, typename OtherElement>
 226 inline void vector2_scale(BasicVector2<Element>& self, OtherElement other)
 227 {
 228   self.x() *= Element(other);
 229   self.y() *= Element(other);
 230 }
 231 template<typename Element, typename OtherElement>
 232 inline void operator*=(BasicVector2<Element>& self, OtherElement other)
 233 {
 234   vector2_scale(self, other);
 235 }
 236
 237
 238 template<typename Element, typename OtherElement>
 239 inline BasicVector2<Element> vector2_scaled(const BasicVector2<Element>& self, const BasicVector2<OtherElement>& other)
 240 {
 241   return BasicVector2<Element>(
 242     Element(self.x() * other.x()),
 243     Element(self.y() * other.y())
 244   );
 245 }
 246 template<typename Element, typename OtherElement>
 247 inline BasicVector2<Element> operator*(const BasicVector2<Element>& self, const BasicVector2<OtherElement>& other)
 248 {
 249   return vector2_scaled(self, other);
 250 }
 251 template<typename Element, typename OtherElement>
 252 inline void vector2_scale(BasicVector2<Element>& self, const BasicVector2<OtherElement>& other)
 253 {
 254   self.x() *= Element(other.x());
 255   self.y() *= Element(other.y());
 256 }
 257 template<typename Element, typename OtherElement>
 258 inline void operator*=(BasicVector2<Element>& self, const BasicVector2<OtherElement>& other)
 259 {
 260   vector2_scale(self, other);
 261 }
 262
 263 template<typename Element, typename OtherElement>
 264 inline BasicVector2<Element> vector2_divided(const BasicVector2<Element>& self, const BasicVector2<OtherElement>& other)
 265 {
 266   return BasicVector2<Element>(
 267     Element(self.x() / other.x()),
 268     Element(self.y() / other.y())
 269   );
 270 }
 271 template<typename Element, typename OtherElement>
 272 inline BasicVector2<Element> operator/(const BasicVector2<Element>& self, const BasicVector2<OtherElement>& other)
 273 {
 274   return vector2_divided(self, other);
 275 }
 276 template<typename Element, typename OtherElement>
 277 inline void vector2_divide(BasicVector2<Element>& self, const BasicVector2<OtherElement>& other)
 278 {
 279   self.x() /= Element(other.x());
 280   self.y() /= Element(other.y());
 281 }
 282 template<typename Element, typename OtherElement>
 283 inline void operator/=(BasicVector2<Element>& self, const BasicVector2<OtherElement>& other)
 284 {
 285   vector2_divide(self, other);
 286 }
 287
 288
 289 template<typename Element, typename OtherElement>
 290 inline BasicVector2<Element> vector2_divided(const BasicVector2<Element>& self, OtherElement other)
 291 {
 292   return BasicVector2<Element>(
 293     Element(self.x() / other),
 294     Element(self.y() / other)
 295   );
 296 }
 297 template<typename Element, typename OtherElement>
 298 inline BasicVector2<Element> operator/(const BasicVector2<Element>& self, OtherElement other)
 299 {
 300   return vector2_divided(self, other);
 301 }
 302 template<typename Element, typename OtherElement>
 303 inline void vector2_divide(BasicVector2<Element>& self, OtherElement other)
 304 {
 305   self.x() /= Element(other);
 306   self.y() /= Element(other);
 307 }
 308 template<typename Element, typename OtherElement>
 309 inline void operator/=(BasicVector2<Element>& self, OtherElement other)
 310 {
 311   vector2_divide(self, other);
 312 }
 313
 314 template<typename Element, typename OtherElement>
 315 inline double vector2_dot(const BasicVector2<Element>& self, const BasicVector2<OtherElement>& other)
 316 {
 317   return self.x() * other.x() + self.y() * other.y();
 318 }
 319
 320 template<typename Element>
 321 inline double vector2_length_squared(const BasicVector2<Element>& self)
 322 {
 323   return vector2_dot(self, self);
 324 }
 325
 326 template<typename Element>
 327 inline double vector2_length(const BasicVector2<Element>& self)
 328 {
 329   return sqrt(vector2_length_squared(self));
 330 }
 331
 332 template<typename Element, typename OtherElement>
 333 inline double vector2_cross(const BasicVector2<Element>& self, const BasicVector2<OtherElement>& other)
 334 {
 335   return self.x() * other.y() - self.y() * other.x();
 336 }
 337
 338 typedef BasicVector2<float> Vector2;
 339
 340 /// \brief A 3-element vector.
 341 template<typename Element>
 342 class BasicVector3
 343 {
 344   Element m_elements[3];
 345 public:
 346
 347   BasicVector3()
 348   {
 349   }
 350   template<typename OtherElement>
 351   BasicVector3(const BasicVector3<OtherElement>& other)
 352   {
 353     x() = static_cast<Element>(other.x());
 354     y() = static_cast<Element>(other.y());
 355     z() = static_cast<Element>(other.z());
 356   }
 357   BasicVector3(const Element& x_, const Element& y_, const Element& z_)
 358   {
 359     x() = x_;
 360     y() = y_;
 361     z() = z_;
 362   }
 363
 364   Element& x()
 365   {
 366     return m_elements[0];
 367   }
 368   const Element& x() const
 369   {
 370     return m_elements[0];
 371   }
 372   Element& y()
 373   {
 374     return m_elements[1];
 375   }
 376   const Element& y() const
 377   {
 378     return m_elements[1];
 379   }
 380   Element& z()
 381   {
 382     return m_elements[2];
 383   }
 384   const Element& z() const
 385   {
 386     return m_elements[2];
 387   }
 388
 389   const Element& operator[](std::size_t i) const
 390   {
 391     return m_elements[i];
 392   }
 393   Element& operator[](std::size_t i)
 394   {
 395     return m_elements[i];
 396   }
 397
 398   operator BasicVector2<Element>&()
 399   {
 400     return reinterpret_cast<BasicVector2<Element>&>(*this);
 401   }
 402   operator const BasicVector2<Element>&() const
 403   {
 404     return reinterpret_cast<const BasicVector2<Element>&>(*this);
 405   }
 406 };
 407
 408 /// \brief A 3-element vector stored in single-precision floating-point.
 409 typedef BasicVector3<float> Vector3;
 410
 411 const Vector3 g_vector3_identity(0, 0, 0);
 412 const Vector3 g_vector3_max = Vector3(FLT_MAX, FLT_MAX, FLT_MAX);
 413 const Vector3 g_vector3_axis_x(1, 0, 0);
 414 const Vector3 g_vector3_axis_y(0, 1, 0);
 415 const Vector3 g_vector3_axis_z(0, 0, 1);
 416
 417 const Vector3 g_vector3_axes[3] = { g_vector3_axis_x, g_vector3_axis_y, g_vector3_axis_z };
 418
 419 template<typename Element>
 420 inline Element* vector3_to_array(BasicVector3<Element>& self)
 421 {
 422   return reinterpret_cast<Element*>(&self);
 423 }
 424 template<typename Element>
 425 inline const Element* vector3_to_array(const BasicVector3<Element>& self)
 426 {
 427   return reinterpret_cast<const Element*>(&self);
 428 }
 429
 430 template<typename Element, typename OtherElement>
 431 inline void vector3_swap(BasicVector3<Element>& self, BasicVector3<OtherElement>& other)
 432 {
 433   std::swap(self.x(), other.x());
 434   std::swap(self.y(), other.y());
 435   std::swap(self.z(), other.z());
 436 }
 437
 438 template<typename Element, typename OtherElement>
 439 inline bool vector3_equal(const BasicVector3<Element>& self, const BasicVector3<OtherElement>& other)
 440 {
 441   return self.x() == other.x() && self.y() == other.y() && self.z() == other.z();
 442 }
 443 template<typename Element, typename OtherElement>
 444 inline bool operator==(const BasicVector3<Element>& self, const BasicVector3<OtherElement>& other)
 445 {
 446   return vector3_equal(self, other);
 447 }
 448 template<typename Element, typename OtherElement>
 449 inline bool operator!=(const BasicVector3<Element>& self, const BasicVector3<OtherElement>& other)
 450 {
 451   return !vector3_equal(self, other);
 452 }
 453
 454
 455 template<typename Element, typename OtherElement, typename Epsilon>
 456 inline bool vector3_equal_epsilon(const BasicVector3<Element>& self, const BasicVector3<OtherElement>& other, Epsilon epsilon)
 457 {
 458   return float_equal_epsilon(self.x(), other.x(), epsilon)
 459     && float_equal_epsilon(self.y(), other.y(), epsilon)
 460     && float_equal_epsilon(self.z(), other.z(), epsilon);
 461 }
 462
 463
 464
 465 template<typename Element, typename OtherElement>
 466 inline BasicVector3<Element> vector3_added(const BasicVector3<Element>& self, const BasicVector3<OtherElement>& other)
 467 {
 468   return BasicVector3<Element>(
 469     Element(self.x() + other.x()),
 470     Element(self.y() + other.y()),
 471     Element(self.z() + other.z())
 472   );
 473 }
 474 template<typename Element, typename OtherElement>
 475 inline BasicVector3<Element> operator+(const BasicVector3<Element>& self, const BasicVector3<OtherElement>& other)
 476 {
 477   return vector3_added(self, other);
 478 }
 479 template<typename Element, typename OtherElement>
 480 inline void vector3_add(BasicVector3<Element>& self, const BasicVector3<OtherElement>& other)
 481 {
 482   self.x() += static_cast<Element>(other.x());
 483   self.y() += static_cast<Element>(other.y());
 484   self.z() += static_cast<Element>(other.z());
 485 }
 486 template<typename Element, typename OtherElement>
 487 inline void operator+=(BasicVector3<Element>& self, const BasicVector3<OtherElement>& other)
 488 {
 489   vector3_add(self, other);
 490 }
 491
 492 template<typename Element, typename OtherElement>
 493 inline BasicVector3<Element> vector3_subtracted(const BasicVector3<Element>& self, const BasicVector3<OtherElement>& other)
 494 {
 495   return BasicVector3<Element>(
 496     Element(self.x() - other.x()),
 497     Element(self.y() - other.y()),
 498     Element(self.z() - other.z())
 499   );
 500 }
 501 template<typename Element, typename OtherElement>
 502 inline BasicVector3<Element> operator-(const BasicVector3<Element>& self, const BasicVector3<OtherElement>& other)
 503 {
 504   return vector3_subtracted(self, other);
 505 }
 506 template<typename Element, typename OtherElement>
 507 inline void vector3_subtract(BasicVector3<Element>& self, const BasicVector3<OtherElement>& other)
 508 {
 509   self.x() -= static_cast<Element>(other.x());
 510   self.y() -= static_cast<Element>(other.y());
 511   self.z() -= static_cast<Element>(other.z());
 512 }
 513 template<typename Element, typename OtherElement>
 514 inline void operator-=(BasicVector3<Element>& self, const BasicVector3<OtherElement>& other)
 515 {
 516   vector3_subtract(self, other);
 517 }
 518
 519 template<typename Element, typename OtherElement>
 520 inline BasicVector3<Element> vector3_scaled(const BasicVector3<Element>& self, const BasicVector3<OtherElement>& other)
 521 {
 522   return BasicVector3<Element>(
 523     Element(self.x() * other.x()),
 524     Element(self.y() * other.y()),
 525     Element(self.z() * other.z())
 526   );
 527 }
 528 template<typename Element, typename OtherElement>
 529 inline BasicVector3<Element> operator*(const BasicVector3<Element>& self, const BasicVector3<OtherElement>& other)
 530 {
 531   return vector3_scaled(self, other);
 532 }
 533 template<typename Element, typename OtherElement>
 534 inline void vector3_scale(BasicVector3<Element>& self, const BasicVector3<OtherElement>& other)
 535 {
 536   self.x() *= static_cast<Element>(other.x());
 537   self.y() *= static_cast<Element>(other.y());
 538   self.z() *= static_cast<Element>(other.z());
 539 }
 540 template<typename Element, typename OtherElement>
 541 inline void operator*=(BasicVector3<Element>& self, const BasicVector3<OtherElement>& other)
 542 {
 543   vector3_scale(self, other);
 544 }
 545
 546 template<typename Element, typename OtherElement>
 547 inline BasicVector3<Element> vector3_scaled(const BasicVector3<Element>& self, const OtherElement& scale)
 548 {
 549   return BasicVector3<Element>(
 550     Element(self.x() * scale),
 551     Element(self.y() * scale),
 552     Element(self.z() * scale)
 553   );
 554 }
 555 template<typename Element, typename OtherElement>
 556 inline BasicVector3<Element> operator*(const BasicVector3<Element>& self, const OtherElement& scale)
 557 {
 558   return vector3_scaled(self, scale);
 559 }
 560 template<typename Element, typename OtherElement>
 561 inline void vector3_scale(BasicVector3<Element>& self, const OtherElement& scale)
 562 {
 563   self.x() *= static_cast<Element>(scale);
 564   self.y() *= static_cast<Element>(scale);
 565   self.z() *= static_cast<Element>(scale);
 566 }
 567 template<typename Element, typename OtherElement>
 568 inline void operator*=(BasicVector3<Element>& self, const OtherElement& scale)
 569 {
 570   vector3_scale(self, scale);
 571 }
 572
 573 template<typename Element, typename OtherElement>
 574 inline BasicVector3<Element> vector3_divided(const BasicVector3<Element>& self, const BasicVector3<OtherElement>& other)
 575 {
 576   return BasicVector3<Element>(
 577     Element(self.x() / other.x()),
 578     Element(self.y() / other.y()),
 579     Element(self.z() / other.z())
 580   );
 581 }
 582 template<typename Element, typename OtherElement>
 583 inline BasicVector3<Element> operator/(const BasicVector3<Element>& self, const BasicVector3<OtherElement>& other)
 584 {
 585   return vector3_divided(self, other);
 586 }
 587 template<typename Element, typename OtherElement>
 588 inline void vector3_divide(BasicVector3<Element>& self, const BasicVector3<OtherElement>& other)
 589 {
 590   self.x() /= static_cast<Element>(other.x());
 591   self.y() /= static_cast<Element>(other.y());
 592   self.z() /= static_cast<Element>(other.z());
 593 }
 594 template<typename Element, typename OtherElement>
 595 inline void operator/=(BasicVector3<Element>& self, const BasicVector3<OtherElement>& other)
 596 {
 597   vector3_divide(self, other);
 598 }
 599
 600 template<typename Element, typename OtherElement>
 601 inline BasicVector3<Element> vector3_divided(const BasicVector3<Element>& self, const OtherElement& divisor)
 602 {
 603   return BasicVector3<Element>(
 604     Element(self.x() / divisor),
 605     Element(self.y() / divisor),
 606     Element(self.z() / divisor)
 607   );
 608 }
 609 template<typename Element, typename OtherElement>
 610 inline BasicVector3<Element> operator/(const BasicVector3<Element>& self, const OtherElement& divisor)
 611 {
 612   return vector3_divided(self, divisor);
 613 }
 614 template<typename Element, typename OtherElement>
 615 inline void vector3_divide(BasicVector3<Element>& self, const OtherElement& divisor)
 616 {
 617   self.x() /= static_cast<Element>(divisor);
 618   self.y() /= static_cast<Element>(divisor);
 619   self.z() /= static_cast<Element>(divisor);
 620 }
 621 template<typename Element, typename OtherElement>
 622 inline void operator/=(BasicVector3<Element>& self, const OtherElement& divisor)
 623 {
 624   vector3_divide(self, divisor);
 625 }
 626
 627 template<typename Element, typename OtherElement>
 628 inline double vector3_dot(const BasicVector3<Element>& self, const BasicVector3<OtherElement>& other)
 629 {
 630   return self.x() * other.x() + self.y() * other.y() + self.z() * other.z();
 631 }
 632
 633 template<typename Element>
 634 inline BasicVector3<Element> vector3_mid(const BasicVector3<Element>& begin, const BasicVector3<Element>& end)
 635 {
 636   return vector3_scaled(vector3_added(begin, end), 0.5);
 637 }
 638
 639 template<typename Element, typename OtherElement>
 640 inline BasicVector3<Element> vector3_cross(const BasicVector3<Element>& self, const BasicVector3<OtherElement>& other)
 641 {
 642   return BasicVector3<Element>(
 643     Element(self.y() * other.z() - self.z() * other.y()),
 644     Element(self.z() * other.x() - self.x() * other.z()),
 645     Element(self.x() * other.y() - self.y() * other.x())
 646   );
 647 }
 648
 649 template<typename Element>
 650 inline BasicVector3<Element> vector3_negated(const BasicVector3<Element>& self)
 651 {
 652   return BasicVector3<Element>(-self.x(), -self.y(), -self.z());
 653 }
 654 template<typename Element>
 655 inline BasicVector3<Element> operator-(const BasicVector3<Element>& self)
 656 {
 657   return vector3_negated(self);
 658 }
 659
 660 template<typename Element>
 661 inline void vector3_negate(BasicVector3<Element>& self)
 662 {
 663   self = vector3_negated(self);
 664 }
 665
 666 template<typename Element>
 667 inline double vector3_length_squared(const BasicVector3<Element>& self)
 668 {
 669   return vector3_dot(self, self);
 670 }
 671
 672 template<typename Element>
 673 inline double vector3_length(const BasicVector3<Element>& self)
 674 {
 675   return sqrt(vector3_length_squared(self));
 676 }
 677
 678 template<typename Element>
 679 inline Element float_divided(Element f, Element other)
 680 {
 681   //ASSERT_MESSAGE(other != 0, "float_divided: invalid divisor");
 682   return f / other;
 683 }
 684
 685 template<typename Element>
 686 inline BasicVector3<Element> vector3_normalised(const BasicVector3<Element>& self)
 687 {
 688   return vector3_scaled(self, float_divided(1.0, vector3_length(self)));
 689 }
 690
 691 template<typename Element>
 692 inline void vector3_normalise(BasicVector3<Element>& self)
 693 {
 694   self = vector3_normalised(self);
 695 }
 696
 697
 698 template<typename Element>
 699 inline BasicVector3<Element> vector3_snapped(const BasicVector3<Element>& self)
 700 {
 701   return BasicVector3<Element>(
 702     Element(float_to_integer(self.x())),
 703     Element(float_to_integer(self.y())),
 704     Element(float_to_integer(self.z()))
 705   );
 706 }
 707 template<typename Element>
 708 inline void vector3_snap(BasicVector3<Element>& self)
 709 {
 710   self = vector3_snapped(self);
 711 }
 712 template<typename Element, typename OtherElement>
 713 inline BasicVector3<Element> vector3_snapped(const BasicVector3<Element>& self, const OtherElement& snap)
 714 {
 715   return BasicVector3<Element>(
 716     Element(float_snapped(self.x(), snap)),
 717     Element(float_snapped(self.y(), snap)),
 718     Element(float_snapped(self.z(), snap))
 719   );
 720 }
 721 template<typename Element, typename OtherElement>
 722 inline void vector3_snap(BasicVector3<Element>& self, const OtherElement& snap)
 723 {
 724   self = vector3_snapped(self, snap);
 725 }
 726
 727 inline Vector3 vector3_for_spherical(double theta, double phi)
 728 {
 729   return Vector3(
 730     static_cast<float>(cos(theta) * cos(phi)),
 731     static_cast<float>(sin(theta) * cos(phi)),
 732     static_cast<float>(sin(phi))
 733   );
 734 }
 735
 736
 737 /// \brief A 4-element vector.
 738 template<typename Element>
 739 class BasicVector4
 740 {
 741   Element m_elements[4];
 742 public:
 743
 744   BasicVector4()
 745   {
 746   }
 747   BasicVector4(Element x_, Element y_, Element z_, Element w_)
 748   {
 749     x() = x_;
 750     y() = y_;
 751     z() = z_;
 752     w() = w_;
 753   }
 754   BasicVector4(const BasicVector3<Element>& self, Element w_)
 755   {
 756     x() = self.x();
 757     y() = self.y();
 758     z() = self.z();
 759     w() = w_;
 760   }
 761
 762   Element& x()
 763   {
 764     return m_elements[0];
 765   }
 766   Element x() const
 767   {
 768     return m_elements[0];
 769   }
 770   Element& y()
 771   {
 772     return m_elements[1];
 773   }
 774   Element y() const
 775   {
 776     return m_elements[1];
 777   }
 778   Element& z()
 779   {
 780     return m_elements[2];
 781   }
 782   Element z() const
 783   {
 784     return m_elements[2];
 785   }
 786   Element& w()
 787   {
 788     return m_elements[3];
 789   }
 790   Element w() const
 791   {
 792     return m_elements[3];
 793   }
 794
 795   Element index(std::size_t i) const
 796   {
 797     return m_elements[i];
 798   }
 799   Element& index(std::size_t i)
 800   {
 801     return m_elements[i];
 802   }
 803   Element operator[](std::size_t i) const
 804   {
 805     return m_elements[i];
 806   }
 807   Element& operator[](std::size_t i)
 808   {
 809     return m_elements[i];
 810   }
 811
 812   operator BasicVector3<Element>&()
 813   {
 814     return reinterpret_cast<BasicVector3<Element>&>(*this);
 815   }
 816   operator const BasicVector3<Element>&() const
 817   {
 818     return reinterpret_cast<const BasicVector3<Element>&>(*this);
 819   }
 820
 821   bool operator==(const BasicVector4& other) const
 822   {
 823     return x() == other.x() && y() == other.y() && z() == other.z() && w() == other.w();
 824   }
 825 };
 826
 827 /// \brief A 4-element vector stored in single-precision floating-point.
 828 typedef BasicVector4<float> Vector4;
 829
 830
 831 template<typename Element, typename OtherElement>
 832 inline bool vector4_equal(const BasicVector4<Element>& self, const BasicVector4<OtherElement>& other)
 833 {
 834   return self.x() == other.x() && self.y() == other.y() && self.z() == other.z() && self.w() == other.w();
 835 }
 836 template<typename Element, typename OtherElement>
 837 inline bool operator==(const BasicVector4<Element>& self, const BasicVector4<OtherElement>& other)
 838 {
 839   return vector4_equal(self, other);
 840 }
 841 template<typename Element, typename OtherElement>
 842 inline bool operator!=(const BasicVector4<Element>& self, const BasicVector4<OtherElement>& other)
 843 {
 844   return !vector4_equal(self, other);
 845 }
 846
 847 template<typename Element, typename OtherElement>
 848 inline bool vector4_equal_epsilon(const BasicVector4<Element>& self, const BasicVector4<OtherElement>& other, Element epsilon)
 849 {
 850   return float_equal_epsilon(self.x(), other.x(), epsilon)
 851     && float_equal_epsilon(self.y(), other.y(), epsilon)
 852     && float_equal_epsilon(self.z(), other.z(), epsilon)
 853     && float_equal_epsilon(self.w(), other.w(), epsilon);
 854 }
 855
 856 template<typename Element>
 857 inline Element* vector4_to_array(BasicVector4<Element>& self)
 858 {
 859   return reinterpret_cast<Element*>(&self);
 860 }
 861 template<typename Element>
 862 inline const float* vector4_to_array(const BasicVector4<Element>& self)
 863 {
 864   return reinterpret_cast<const Element*>(&self);
 865 }
 866
 867 template<typename Element>
 868 inline Vector3& vector4_to_vector3(BasicVector4<Element>& self)
 869 {
 870   return reinterpret_cast<BasicVector3<Element>&>(self);
 871 }
 872 template<typename Element>
 873 inline const Vector3& vector4_to_vector3(const BasicVector4<Element>& self)
 874 {
 875   return reinterpret_cast<const BasicVector3<Element>&>(self);
 876 }
 877
 878 template<typename Element, typename OtherElement>
 879 inline BasicVector4<Element> vector4_added(const BasicVector4<Element>& self, const BasicVector4<OtherElement>& other)
 880 {
 881   return BasicVector4<Element>(
 882     float(self.x() + other.x()),
 883     float(self.y() + other.y()),
 884     float(self.z() + other.z()),
 885     float(self.w() + other.w())
 886   );
 887 }
 888 template<typename Element, typename OtherElement>
 889 inline BasicVector4<Element> operator+(const BasicVector4<Element>& self, const BasicVector4<OtherElement>& other)
 890 {
 891   return vector4_added(self, other);
 892 }
 893 template<typename Element, typename OtherElement>
 894 inline void vector4_add(BasicVector4<Element>& self, const BasicVector4<OtherElement>& other)
 895 {
 896   self.x() += static_cast<float>(other.x());
 897   self.y() += static_cast<float>(other.y());
 898   self.z() += static_cast<float>(other.z());
 899   self.w() += static_cast<float>(other.w());
 900 }
 901 template<typename Element, typename OtherElement>
 902 inline void operator+=(BasicVector4<Element>& self, const BasicVector4<OtherElement>& other)
 903 {
 904   vector4_add(self, other);
 905 }
 906
 907 template<typename Element, typename OtherElement>
 908 inline BasicVector4<Element> vector4_subtracted(const BasicVector4<Element>& self, const BasicVector4<OtherElement>& other)
 909 {
 910   return BasicVector4<Element>(
 911     float(self.x() - other.x()),
 912     float(self.y() - other.y()),
 913     float(self.z() - other.z()),
 914     float(self.w() - other.w())
 915   );
 916 }
 917 template<typename Element, typename OtherElement>
 918 inline BasicVector4<Element> operator-(const BasicVector4<Element>& self, const BasicVector4<OtherElement>& other)
 919 {
 920   return vector4_subtracted(self, other);
 921 }
 922 template<typename Element, typename OtherElement>
 923 inline void vector4_subtract(BasicVector4<Element>& self, const BasicVector4<OtherElement>& other)
 924 {
 925   self.x() -= static_cast<float>(other.x());
 926   self.y() -= static_cast<float>(other.y());
 927   self.z() -= static_cast<float>(other.z());
 928   self.w() -= static_cast<float>(other.w());
 929 }
 930 template<typename Element, typename OtherElement>
 931 inline void operator-=(BasicVector4<Element>& self, const BasicVector4<OtherElement>& other)
 932 {
 933   vector4_subtract(self, other);
 934 }
 935
 936 template<typename Element, typename OtherElement>
 937 inline BasicVector4<Element> vector4_scaled(const BasicVector4<Element>& self, const BasicVector4<OtherElement>& other)
 938 {
 939   return BasicVector4<Element>(
 940     float(self.x() * other.x()),
 941     float(self.y() * other.y()),
 942     float(self.z() * other.z()),
 943     float(self.w() * other.w())
 944   );
 945 }
 946 template<typename Element, typename OtherElement>
 947 inline BasicVector4<Element> operator*(const BasicVector4<Element>& self, const BasicVector4<OtherElement>& other)
 948 {
 949   return vector4_scaled(self, other);
 950 }
 951 template<typename Element, typename OtherElement>
 952 inline void vector4_scale(BasicVector4<Element>& self, const BasicVector4<OtherElement>& other)
 953 {
 954   self.x() *= static_cast<float>(other.x());
 955   self.y() *= static_cast<float>(other.y());
 956   self.z() *= static_cast<float>(other.z());
 957   self.w() *= static_cast<float>(other.w());
 958 }
 959 template<typename Element, typename OtherElement>
 960 inline void operator*=(BasicVector4<Element>& self, const BasicVector4<OtherElement>& other)
 961 {
 962   vector4_scale(self, other);
 963 }
 964
 965 template<typename Element, typename OtherElement>
 966 inline BasicVector4<Element> vector4_scaled(const BasicVector4<Element>& self, OtherElement scale)
 967 {
 968   return BasicVector4<Element>(
 969     float(self.x() * scale),
 970     float(self.y() * scale),
 971     float(self.z() * scale),
 972     float(self.w() * scale)
 973   );
 974 }
 975 template<typename Element, typename OtherElement>
 976 inline BasicVector4<Element> operator*(const BasicVector4<Element>& self, OtherElement scale)
 977 {
 978   return vector4_scaled(self, scale);
 979 }
 980 template<typename Element, typename OtherElement>
 981 inline void vector4_scale(BasicVector4<Element>& self, OtherElement scale)
 982 {
 983   self.x() *= static_cast<float>(scale);
 984   self.y() *= static_cast<float>(scale);
 985   self.z() *= static_cast<float>(scale);
 986   self.w() *= static_cast<float>(scale);
 987 }
 988 template<typename Element, typename OtherElement>
 989 inline void operator*=(BasicVector4<Element>& self, OtherElement scale)
 990 {
 991   vector4_scale(self, scale);
 992 }
 993
 994 template<typename Element, typename OtherElement>
 995 inline BasicVector4<Element> vector4_divided(const BasicVector4<Element>& self, OtherElement divisor)
 996 {
 997   return BasicVector4<Element>(
 998     float(self.x() / divisor),
 999     float(self.y() / divisor),
1000     float(self.z() / divisor),
1001     float(self.w() / divisor)
1002   );
1003 }
1004 template<typename Element, typename OtherElement>
1005 inline BasicVector4<Element> operator/(const BasicVector4<Element>& self, OtherElement divisor)
1006 {
1007   return vector4_divided(self, divisor);
1008 }
1009 template<typename Element, typename OtherElement>
1010 inline void vector4_divide(BasicVector4<Element>& self, OtherElement divisor)
1011 {
1012   self.x() /= divisor;
1013   self.y() /= divisor;
1014   self.z() /= divisor;
1015   self.w() /= divisor;
1016 }
1017 template<typename Element, typename OtherElement>
1018 inline void operator/=(BasicVector4<Element>& self, OtherElement divisor)
1019 {
1020   vector4_divide(self, divisor);
1021 }
1022
1023 template<typename Element, typename OtherElement>
1024 inline double vector4_dot(const BasicVector4<Element>& self, const BasicVector4<OtherElement>& other)
1025 {
1026   return self.x() * other.x() + self.y() * other.y() + self.z() * other.z() + self.w() * other.w();
1027 }
1028
1029 template<typename Element>
1030 inline BasicVector3<Element> vector4_projected(const BasicVector4<Element>& self)
1031 {
1032   return vector3_scaled(vector4_to_vector3(self), 1.0 / self[3]);
1033 }
1034
1035 #endif