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
 655 template<typename Element>
 656 inline void vector3_negate(BasicVector3<Element>& self)
 657 {
 658   self = vector3_negated(self);
 659 }
 660
 661 template<typename Element>
 662 inline double vector3_length_squared(const BasicVector3<Element>& self)
 663 {
 664   return vector3_dot(self, self);
 665 }
 666
 667 template<typename Element>
 668 inline double vector3_length(const BasicVector3<Element>& self)
 669 {
 670   return sqrt(vector3_length_squared(self));
 671 }
 672
 673 template<typename Element>
 674 inline Element float_divided(Element f, Element other)
 675 {
 676   //ASSERT_MESSAGE(other != 0, "float_divided: invalid divisor");
 677   return f / other;
 678 }
 679
 680 template<typename Element>
 681 inline BasicVector3<Element> vector3_normalised(const BasicVector3<Element>& self)
 682 {
 683   return vector3_scaled(self, float_divided(1.0, vector3_length(self)));
 684 }
 685
 686 template<typename Element>
 687 inline void vector3_normalise(BasicVector3<Element>& self)
 688 {
 689   self = vector3_normalised(self);
 690 }
 691
 692
 693 template<typename Element>
 694 inline BasicVector3<Element> vector3_snapped(const BasicVector3<Element>& self)
 695 {
 696   return BasicVector3<Element>(
 697     Element(float_to_integer(self.x())),
 698     Element(float_to_integer(self.y())),
 699     Element(float_to_integer(self.z()))
 700   );
 701 }
 702 template<typename Element>
 703 inline void vector3_snap(BasicVector3<Element>& self)
 704 {
 705   self = vector3_snapped(self);
 706 }
 707 template<typename Element, typename OtherElement>
 708 inline BasicVector3<Element> vector3_snapped(const BasicVector3<Element>& self, const OtherElement& snap)
 709 {
 710   return BasicVector3<Element>(
 711     Element(float_snapped(self.x(), snap)),
 712     Element(float_snapped(self.y(), snap)),
 713     Element(float_snapped(self.z(), snap))
 714   );
 715 }
 716 template<typename Element, typename OtherElement>
 717 inline void vector3_snap(BasicVector3<Element>& self, const OtherElement& snap)
 718 {
 719   self = vector3_snapped(self, snap);
 720 }
 721
 722 inline Vector3 vector3_for_spherical(double theta, double phi)
 723 {
 724   return Vector3(
 725     static_cast<float>(cos(theta) * cos(phi)),
 726     static_cast<float>(sin(theta) * cos(phi)),
 727     static_cast<float>(sin(phi))
 728   );
 729 }
 730
 731
 732 /// \brief A 4-element vector.
 733 template<typename Element>
 734 class BasicVector4
 735 {
 736   Element m_elements[4];
 737 public:
 738
 739   BasicVector4()
 740   {
 741   }
 742   BasicVector4(Element x_, Element y_, Element z_, Element w_)
 743   {
 744     x() = x_;
 745     y() = y_;
 746     z() = z_;
 747     w() = w_;
 748   }
 749   BasicVector4(const BasicVector3<Element>& self, Element w_)
 750   {
 751     x() = self.x();
 752     y() = self.y();
 753     z() = self.z();
 754     w() = w_;
 755   }
 756
 757   Element& x()
 758   {
 759     return m_elements[0];
 760   }
 761   Element x() const
 762   {
 763     return m_elements[0];
 764   }
 765   Element& y()
 766   {
 767     return m_elements[1];
 768   }
 769   Element y() const
 770   {
 771     return m_elements[1];
 772   }
 773   Element& z()
 774   {
 775     return m_elements[2];
 776   }
 777   Element z() const
 778   {
 779     return m_elements[2];
 780   }
 781   Element& w()
 782   {
 783     return m_elements[3];
 784   }
 785   Element w() const
 786   {
 787     return m_elements[3];
 788   }
 789
 790   Element index(std::size_t i) const
 791   {
 792     return m_elements[i];
 793   }
 794   Element& index(std::size_t i)
 795   {
 796     return m_elements[i];
 797   }
 798   Element operator[](std::size_t i) const
 799   {
 800     return m_elements[i];
 801   }
 802   Element& operator[](std::size_t i)
 803   {
 804     return m_elements[i];
 805   }
 806
 807   operator BasicVector3<Element>&()
 808   {
 809     return reinterpret_cast<BasicVector3<Element>&>(*this);
 810   }
 811   operator const BasicVector3<Element>&() const
 812   {
 813     return reinterpret_cast<const BasicVector3<Element>&>(*this);
 814   }
 815
 816   bool operator==(const BasicVector4& other) const
 817   {
 818     return x() == other.x() && y() == other.y() && z() == other.z() && w() == other.w();
 819   }
 820 };
 821
 822 /// \brief A 4-element vector stored in single-precision floating-point.
 823 typedef BasicVector4<float> Vector4;
 824
 825
 826 template<typename Element, typename OtherElement>
 827 inline bool vector4_equal(const BasicVector4<Element>& self, const BasicVector4<OtherElement>& other)
 828 {
 829   return self.x() == other.x() && self.y() == other.y() && self.z() == other.z() && self.w() == other.w();
 830 }
 831 template<typename Element, typename OtherElement>
 832 inline bool operator==(const BasicVector4<Element>& self, const BasicVector4<OtherElement>& other)
 833 {
 834   return vector4_equal(self, other);
 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
 842 template<typename Element, typename OtherElement>
 843 inline bool vector4_equal_epsilon(const BasicVector4<Element>& self, const BasicVector4<OtherElement>& other, Element epsilon)
 844 {
 845   return float_equal_epsilon(self.x(), other.x(), epsilon)
 846     && float_equal_epsilon(self.y(), other.y(), epsilon)
 847     && float_equal_epsilon(self.z(), other.z(), epsilon)
 848     && float_equal_epsilon(self.w(), other.w(), epsilon);
 849 }
 850
 851 template<typename Element>
 852 inline Element* vector4_to_array(BasicVector4<Element>& self)
 853 {
 854   return reinterpret_cast<Element*>(&self);
 855 }
 856 template<typename Element>
 857 inline const float* vector4_to_array(const BasicVector4<Element>& self)
 858 {
 859   return reinterpret_cast<const Element*>(&self);
 860 }
 861
 862 template<typename Element>
 863 inline Vector3& vector4_to_vector3(BasicVector4<Element>& self)
 864 {
 865   return reinterpret_cast<BasicVector3<Element>&>(self);
 866 }
 867 template<typename Element>
 868 inline const Vector3& vector4_to_vector3(const BasicVector4<Element>& self)
 869 {
 870   return reinterpret_cast<const BasicVector3<Element>&>(self);
 871 }
 872
 873 template<typename Element, typename OtherElement>
 874 inline BasicVector4<Element> vector4_added(const BasicVector4<Element>& self, const BasicVector4<OtherElement>& other)
 875 {
 876   return BasicVector4<Element>(
 877     float(self.x() + other.x()),
 878     float(self.y() + other.y()),
 879     float(self.z() + other.z()),
 880     float(self.w() + other.w())
 881   );
 882 }
 883 template<typename Element, typename OtherElement>
 884 inline BasicVector4<Element> operator+(const BasicVector4<Element>& self, const BasicVector4<OtherElement>& other)
 885 {
 886   return vector4_added(self, other);
 887 }
 888 template<typename Element, typename OtherElement>
 889 inline void vector4_add(BasicVector4<Element>& self, const BasicVector4<OtherElement>& other)
 890 {
 891   self.x() += static_cast<float>(other.x());
 892   self.y() += static_cast<float>(other.y());
 893   self.z() += static_cast<float>(other.z());
 894   self.w() += static_cast<float>(other.w());
 895 }
 896 template<typename Element, typename OtherElement>
 897 inline void operator+=(BasicVector4<Element>& self, const BasicVector4<OtherElement>& other)
 898 {
 899   vector4_add(self, other);
 900 }
 901
 902 template<typename Element, typename OtherElement>
 903 inline BasicVector4<Element> vector4_subtracted(const BasicVector4<Element>& self, const BasicVector4<OtherElement>& other)
 904 {
 905   return BasicVector4<Element>(
 906     float(self.x() - other.x()),
 907     float(self.y() - other.y()),
 908     float(self.z() - other.z()),
 909     float(self.w() - other.w())
 910   );
 911 }
 912 template<typename Element, typename OtherElement>
 913 inline BasicVector4<Element> operator-(const BasicVector4<Element>& self, const BasicVector4<OtherElement>& other)
 914 {
 915   return vector4_subtracted(self, other);
 916 }
 917 template<typename Element, typename OtherElement>
 918 inline void vector4_subtract(BasicVector4<Element>& self, const BasicVector4<OtherElement>& other)
 919 {
 920   self.x() -= static_cast<float>(other.x());
 921   self.y() -= static_cast<float>(other.y());
 922   self.z() -= static_cast<float>(other.z());
 923   self.w() -= static_cast<float>(other.w());
 924 }
 925 template<typename Element, typename OtherElement>
 926 inline void operator-=(BasicVector4<Element>& self, const BasicVector4<OtherElement>& other)
 927 {
 928   vector4_subtract(self, other);
 929 }
 930
 931 template<typename Element, typename OtherElement>
 932 inline BasicVector4<Element> vector4_scaled(const BasicVector4<Element>& self, const BasicVector4<OtherElement>& other)
 933 {
 934   return BasicVector4<Element>(
 935     float(self.x() * other.x()),
 936     float(self.y() * other.y()),
 937     float(self.z() * other.z()),
 938     float(self.w() * other.w())
 939   );
 940 }
 941 template<typename Element, typename OtherElement>
 942 inline BasicVector4<Element> operator*(const BasicVector4<Element>& self, const BasicVector4<OtherElement>& other)
 943 {
 944   return vector4_scaled(self, other);
 945 }
 946 template<typename Element, typename OtherElement>
 947 inline void vector4_scale(BasicVector4<Element>& self, const BasicVector4<OtherElement>& other)
 948 {
 949   self.x() *= static_cast<float>(other.x());
 950   self.y() *= static_cast<float>(other.y());
 951   self.z() *= static_cast<float>(other.z());
 952   self.w() *= static_cast<float>(other.w());
 953 }
 954 template<typename Element, typename OtherElement>
 955 inline void operator*=(BasicVector4<Element>& self, const BasicVector4<OtherElement>& other)
 956 {
 957   vector4_scale(self, other);
 958 }
 959
 960 template<typename Element, typename OtherElement>
 961 inline BasicVector4<Element> vector4_scaled(const BasicVector4<Element>& self, OtherElement scale)
 962 {
 963   return BasicVector4<Element>(
 964     float(self.x() * scale),
 965     float(self.y() * scale),
 966     float(self.z() * scale),
 967     float(self.w() * scale)
 968   );
 969 }
 970 template<typename Element, typename OtherElement>
 971 inline BasicVector4<Element> operator*(const BasicVector4<Element>& self, OtherElement scale)
 972 {
 973   return vector4_scaled(self, scale);
 974 }
 975 template<typename Element, typename OtherElement>
 976 inline void vector4_scale(BasicVector4<Element>& self, OtherElement scale)
 977 {
 978   self.x() *= static_cast<float>(scale);
 979   self.y() *= static_cast<float>(scale);
 980   self.z() *= static_cast<float>(scale);
 981   self.w() *= static_cast<float>(scale);
 982 }
 983 template<typename Element, typename OtherElement>
 984 inline void operator*=(BasicVector4<Element>& self, OtherElement scale)
 985 {
 986   vector4_scale(self, scale);
 987 }
 988
 989 template<typename Element, typename OtherElement>
 990 inline BasicVector4<Element> vector4_divided(const BasicVector4<Element>& self, OtherElement divisor)
 991 {
 992   return BasicVector4<Element>(
 993     float(self.x() / divisor),
 994     float(self.y() / divisor),
 995     float(self.z() / divisor),
 996     float(self.w() / divisor)
 997   );
 998 }
 999 template<typename Element, typename OtherElement>
1000 inline BasicVector4<Element> operator/(const BasicVector4<Element>& self, OtherElement divisor)
1001 {
1002   return vector4_divided(self, divisor);
1003 }
1004 template<typename Element, typename OtherElement>
1005 inline void vector4_divide(BasicVector4<Element>& self, OtherElement divisor)
1006 {
1007   self.x() /= divisor;
1008   self.y() /= divisor;
1009   self.z() /= divisor;
1010   self.w() /= divisor;
1011 }
1012 template<typename Element, typename OtherElement>
1013 inline void operator/=(BasicVector4<Element>& self, OtherElement divisor)
1014 {
1015   vector4_divide(self, divisor);
1016 }
1017
1018 template<typename Element, typename OtherElement>
1019 inline double vector4_dot(const BasicVector4<Element>& self, const BasicVector4<OtherElement>& other)
1020 {
1021   return self.x() * other.x() + self.y() * other.y() + self.z() * other.z() + self.w() * other.w();
1022 }
1023
1024 template<typename Element>
1025 inline BasicVector3<Element> vector4_projected(const BasicVector4<Element>& self)
1026 {
1027   return vector3_scaled(vector4_to_vector3(self), 1.0 / self[3]);
1028 }
1029
1030 #endif