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 #include "generic/vector.h"
  29
  30 #if 0
  31
  32 #define lrint(dbl)              ((int)((dbl) + 0.5))
  33 #define lrintf(flt)             ((int)((flt) + 0.5))
  34
  35 #endif
  36
  37 #if defined (_MSC_VER)
  38
  39 inline int lrint (double flt)
  40 {
  41   int i;
  42
  43         _asm
  44         {
  45     fld flt
  46                 fistp i
  47   };
  48
  49         return i;
  50 }
  51
  52 #else // lrint is part of ISO C99
  53
  54 #define _ISOC9X_SOURCE  1
  55 #define _ISOC99_SOURCE  1
  56
  57 #define __USE_ISOC9X    1
  58 #define __USE_ISOC99    1
  59
  60 #endif
  61
  62 #include <cmath>
  63 #include <float.h>
  64 #include <algorithm>
  65
  66 //#include "debugging/debugging.h"
  67
  68 /// \brief Returns true if \p self is equal to other \p other within \p epsilon.
  69 template<typename Element, typename OtherElement>
  70 inline bool float_equal_epsilon(const Element& self, const OtherElement& other, const Element& epsilon)
  71 {
  72   return fabs(other - self) < epsilon;
  73 }
  74
  75 /// \brief Returns the value midway between \p self and \p other.
  76 template<typename Element>
  77 inline Element float_mid(const Element& self, const Element& other)
  78 {
  79   return Element((self + other) * 0.5);
  80 }
  81
  82 /// \brief Returns \p f rounded to the nearest integer. Note that this is not the same behaviour as casting from float to int.
  83 template<typename Element>
  84 inline int float_to_integer(const Element& f)
  85 {
  86   return lrint(f);
  87 }
  88
  89 /// \brief Returns \p f rounded to the nearest multiple of \p snap.
  90 template<typename Element, typename OtherElement>
  91 inline Element float_snapped(const Element& f, const OtherElement& snap)
  92 {
  93   return Element(float_to_integer(f / snap) * snap);
  94 }
  95
  96 /// \brief Returns true if \p f has no decimal fraction part.
  97 template<typename Element>
  98 inline bool float_is_integer(const Element& f)
  99 {
 100   return f == Element(float_to_integer(f));
 101 }
 102
 103 /// \brief Returns \p self modulated by the range [0, \p modulus)
 104 /// \p self must be in the range [\p -modulus, \p modulus)
 105 template<typename Element, typename ModulusElement>
 106 inline Element float_mod_range(const Element& self, const ModulusElement& modulus)
 107 {
 108   return Element((self < 0.0) ? self + modulus : self);
 109 }
 110
 111 /// \brief Returns \p self modulated by the range [0, \p modulus)
 112 template<typename Element, typename ModulusElement>
 113 inline Element float_mod(const Element& self, const ModulusElement& modulus)
 114 {
 115   return float_mod_range(Element(fmod(static_cast<double>(self), static_cast<double>(modulus))), modulus);
 116 }
 117
 118
 119 template<typename Element, typename OtherElement>
 120 inline BasicVector2<Element> vector2_added(const BasicVector2<Element>& self, const BasicVector2<OtherElement>& other)
 121 {
 122   return BasicVector2<Element>(
 123     Element(self.x() + other.x()),
 124     Element(self.y() + other.y())
 125   );
 126 }
 127 template<typename Element, typename OtherElement>
 128 inline BasicVector2<Element> operator+(const BasicVector2<Element>& self, const BasicVector2<OtherElement>& other)
 129 {
 130   return vector2_added(self, other);
 131 }
 132 template<typename Element, typename OtherElement>
 133 inline void vector2_add(BasicVector2<Element>& self, const BasicVector2<OtherElement>& other)
 134 {
 135   self.x() += Element(other.x());
 136   self.y() += Element(other.y());
 137 }
 138 template<typename Element, typename OtherElement>
 139 inline void operator+=(BasicVector2<Element>& self, const BasicVector2<OtherElement>& other)
 140 {
 141   vector2_add(self, other);
 142 }
 143
 144
 145 template<typename Element, typename OtherElement>
 146 inline BasicVector2<Element> vector2_subtracted(const BasicVector2<Element>& self, const BasicVector2<OtherElement>& other)
 147 {
 148   return BasicVector2<Element>(
 149     Element(self.x() - other.x()),
 150     Element(self.y() - other.y())
 151   );
 152 }
 153 template<typename Element, typename OtherElement>
 154 inline BasicVector2<Element> operator-(const BasicVector2<Element>& self, const BasicVector2<OtherElement>& other)
 155 {
 156   return vector2_subtracted(self, other);
 157 }
 158 template<typename Element, typename OtherElement>
 159 inline void vector2_subtract(BasicVector2<Element>& self, const BasicVector2<OtherElement>& other)
 160 {
 161   self.x() -= Element(other.x());
 162   self.y() -= lement(other.y());
 163 }
 164 template<typename Element, typename OtherElement>
 165 inline void operator-=(BasicVector2<Element>& self, const BasicVector2<OtherElement>& other)
 166 {
 167   vector2_subtract(self, other);
 168 }
 169
 170
 171 template<typename Element, typename OtherElement>
 172 inline BasicVector2<Element> vector2_scaled(const BasicVector2<Element>& self, OtherElement other)
 173 {
 174   return BasicVector2<Element>(
 175     Element(self.x() * other),
 176     Element(self.y() * other)
 177   );
 178 }
 179 template<typename Element, typename OtherElement>
 180 inline BasicVector2<Element> operator*(const BasicVector2<Element>& self, OtherElement other)
 181 {
 182   return vector2_scaled(self, other);
 183 }
 184 template<typename Element, typename OtherElement>
 185 inline void vector2_scale(BasicVector2<Element>& self, OtherElement other)
 186 {
 187   self.x() *= Element(other);
 188   self.y() *= Element(other);
 189 }
 190 template<typename Element, typename OtherElement>
 191 inline void operator*=(BasicVector2<Element>& self, OtherElement other)
 192 {
 193   vector2_scale(self, other);
 194 }
 195
 196
 197 template<typename Element, typename OtherElement>
 198 inline BasicVector2<Element> vector2_scaled(const BasicVector2<Element>& self, const BasicVector2<OtherElement>& other)
 199 {
 200   return BasicVector2<Element>(
 201     Element(self.x() * other.x()),
 202     Element(self.y() * other.y())
 203   );
 204 }
 205 template<typename Element, typename OtherElement>
 206 inline BasicVector2<Element> operator*(const BasicVector2<Element>& self, const BasicVector2<OtherElement>& other)
 207 {
 208   return vector2_scaled(self, other);
 209 }
 210 template<typename Element, typename OtherElement>
 211 inline void vector2_scale(BasicVector2<Element>& self, const BasicVector2<OtherElement>& other)
 212 {
 213   self.x() *= Element(other.x());
 214   self.y() *= Element(other.y());
 215 }
 216 template<typename Element, typename OtherElement>
 217 inline void operator*=(BasicVector2<Element>& self, const BasicVector2<OtherElement>& other)
 218 {
 219   vector2_scale(self, other);
 220 }
 221
 222 template<typename Element, typename OtherElement>
 223 inline BasicVector2<Element> vector2_divided(const BasicVector2<Element>& self, const BasicVector2<OtherElement>& other)
 224 {
 225   return BasicVector2<Element>(
 226     Element(self.x() / other.x()),
 227     Element(self.y() / other.y())
 228   );
 229 }
 230 template<typename Element, typename OtherElement>
 231 inline BasicVector2<Element> operator/(const BasicVector2<Element>& self, const BasicVector2<OtherElement>& other)
 232 {
 233   return vector2_divided(self, other);
 234 }
 235 template<typename Element, typename OtherElement>
 236 inline void vector2_divide(BasicVector2<Element>& self, const BasicVector2<OtherElement>& other)
 237 {
 238   self.x() /= Element(other.x());
 239   self.y() /= Element(other.y());
 240 }
 241 template<typename Element, typename OtherElement>
 242 inline void operator/=(BasicVector2<Element>& self, const BasicVector2<OtherElement>& other)
 243 {
 244   vector2_divide(self, other);
 245 }
 246
 247
 248 template<typename Element, typename OtherElement>
 249 inline BasicVector2<Element> vector2_divided(const BasicVector2<Element>& self, OtherElement other)
 250 {
 251   return BasicVector2<Element>(
 252     Element(self.x() / other),
 253     Element(self.y() / other)
 254   );
 255 }
 256 template<typename Element, typename OtherElement>
 257 inline BasicVector2<Element> operator/(const BasicVector2<Element>& self, OtherElement other)
 258 {
 259   return vector2_divided(self, other);
 260 }
 261 template<typename Element, typename OtherElement>
 262 inline void vector2_divide(BasicVector2<Element>& self, OtherElement other)
 263 {
 264   self.x() /= Element(other);
 265   self.y() /= Element(other);
 266 }
 267 template<typename Element, typename OtherElement>
 268 inline void operator/=(BasicVector2<Element>& self, OtherElement other)
 269 {
 270   vector2_divide(self, other);
 271 }
 272
 273 template<typename Element, typename OtherElement>
 274 inline double vector2_dot(const BasicVector2<Element>& self, const BasicVector2<OtherElement>& other)
 275 {
 276   return self.x() * other.x() + self.y() * other.y();
 277 }
 278
 279 template<typename Element>
 280 inline double vector2_length_squared(const BasicVector2<Element>& self)
 281 {
 282   return vector2_dot(self, self);
 283 }
 284
 285 template<typename Element>
 286 inline double vector2_length(const BasicVector2<Element>& self)
 287 {
 288   return sqrt(vector2_length_squared(self));
 289 }
 290
 291 template<typename Element, typename OtherElement>
 292 inline double vector2_cross(const BasicVector2<Element>& self, const BasicVector2<OtherElement>& other)
 293 {
 294   return self.x() * other.y() - self.y() * other.x();
 295 }
 296
 297 const Vector3 g_vector3_identity(0, 0, 0);
 298 const Vector3 g_vector3_max = Vector3(FLT_MAX, FLT_MAX, FLT_MAX);
 299 const Vector3 g_vector3_axis_x(1, 0, 0);
 300 const Vector3 g_vector3_axis_y(0, 1, 0);
 301 const Vector3 g_vector3_axis_z(0, 0, 1);
 302
 303 const Vector3 g_vector3_axes[3] = { g_vector3_axis_x, g_vector3_axis_y, g_vector3_axis_z };
 304
 305 template<typename Element, typename OtherElement>
 306 inline void vector3_swap(BasicVector3<Element>& self, BasicVector3<OtherElement>& other)
 307 {
 308   std::swap(self.x(), other.x());
 309   std::swap(self.y(), other.y());
 310   std::swap(self.z(), other.z());
 311 }
 312
 313 template<typename Element, typename OtherElement>
 314 inline bool vector3_equal(const BasicVector3<Element>& self, const BasicVector3<OtherElement>& other)
 315 {
 316   return self.x() == other.x() && self.y() == other.y() && self.z() == other.z();
 317 }
 318 template<typename Element, typename OtherElement>
 319 inline bool operator==(const BasicVector3<Element>& self, const BasicVector3<OtherElement>& other)
 320 {
 321   return vector3_equal(self, other);
 322 }
 323 template<typename Element, typename OtherElement>
 324 inline bool operator!=(const BasicVector3<Element>& self, const BasicVector3<OtherElement>& other)
 325 {
 326   return !vector3_equal(self, other);
 327 }
 328
 329
 330 template<typename Element, typename OtherElement, typename Epsilon>
 331 inline bool vector3_equal_epsilon(const BasicVector3<Element>& self, const BasicVector3<OtherElement>& other, Epsilon epsilon)
 332 {
 333   return float_equal_epsilon(self.x(), other.x(), epsilon)
 334     && float_equal_epsilon(self.y(), other.y(), epsilon)
 335     && float_equal_epsilon(self.z(), other.z(), epsilon);
 336 }
 337
 338
 339
 340 template<typename Element, typename OtherElement>
 341 inline BasicVector3<Element> vector3_added(const BasicVector3<Element>& self, const BasicVector3<OtherElement>& other)
 342 {
 343   return BasicVector3<Element>(
 344     Element(self.x() + other.x()),
 345     Element(self.y() + other.y()),
 346     Element(self.z() + other.z())
 347   );
 348 }
 349 template<typename Element, typename OtherElement>
 350 inline BasicVector3<Element> operator+(const BasicVector3<Element>& self, const BasicVector3<OtherElement>& other)
 351 {
 352   return vector3_added(self, other);
 353 }
 354 template<typename Element, typename OtherElement>
 355 inline void vector3_add(BasicVector3<Element>& self, const BasicVector3<OtherElement>& other)
 356 {
 357   self.x() += static_cast<Element>(other.x());
 358   self.y() += static_cast<Element>(other.y());
 359   self.z() += static_cast<Element>(other.z());
 360 }
 361 template<typename Element, typename OtherElement>
 362 inline void operator+=(BasicVector3<Element>& self, const BasicVector3<OtherElement>& other)
 363 {
 364   vector3_add(self, other);
 365 }
 366
 367 template<typename Element, typename OtherElement>
 368 inline BasicVector3<Element> vector3_subtracted(const BasicVector3<Element>& self, const BasicVector3<OtherElement>& other)
 369 {
 370   return BasicVector3<Element>(
 371     Element(self.x() - other.x()),
 372     Element(self.y() - other.y()),
 373     Element(self.z() - other.z())
 374   );
 375 }
 376 template<typename Element, typename OtherElement>
 377 inline BasicVector3<Element> operator-(const BasicVector3<Element>& self, const BasicVector3<OtherElement>& other)
 378 {
 379   return vector3_subtracted(self, other);
 380 }
 381 template<typename Element, typename OtherElement>
 382 inline void vector3_subtract(BasicVector3<Element>& self, const BasicVector3<OtherElement>& other)
 383 {
 384   self.x() -= static_cast<Element>(other.x());
 385   self.y() -= static_cast<Element>(other.y());
 386   self.z() -= static_cast<Element>(other.z());
 387 }
 388 template<typename Element, typename OtherElement>
 389 inline void operator-=(BasicVector3<Element>& self, const BasicVector3<OtherElement>& other)
 390 {
 391   vector3_subtract(self, other);
 392 }
 393
 394 template<typename Element, typename OtherElement>
 395 inline BasicVector3<Element> vector3_scaled(const BasicVector3<Element>& self, const BasicVector3<OtherElement>& other)
 396 {
 397   return BasicVector3<Element>(
 398     Element(self.x() * other.x()),
 399     Element(self.y() * other.y()),
 400     Element(self.z() * other.z())
 401   );
 402 }
 403 template<typename Element, typename OtherElement>
 404 inline BasicVector3<Element> operator*(const BasicVector3<Element>& self, const BasicVector3<OtherElement>& other)
 405 {
 406   return vector3_scaled(self, other);
 407 }
 408 template<typename Element, typename OtherElement>
 409 inline void vector3_scale(BasicVector3<Element>& self, const BasicVector3<OtherElement>& other)
 410 {
 411   self.x() *= static_cast<Element>(other.x());
 412   self.y() *= static_cast<Element>(other.y());
 413   self.z() *= static_cast<Element>(other.z());
 414 }
 415 template<typename Element, typename OtherElement>
 416 inline void operator*=(BasicVector3<Element>& self, const BasicVector3<OtherElement>& other)
 417 {
 418   vector3_scale(self, other);
 419 }
 420
 421 template<typename Element, typename OtherElement>
 422 inline BasicVector3<Element> vector3_scaled(const BasicVector3<Element>& self, const OtherElement& scale)
 423 {
 424   return BasicVector3<Element>(
 425     Element(self.x() * scale),
 426     Element(self.y() * scale),
 427     Element(self.z() * scale)
 428   );
 429 }
 430 template<typename Element, typename OtherElement>
 431 inline BasicVector3<Element> operator*(const BasicVector3<Element>& self, const OtherElement& scale)
 432 {
 433   return vector3_scaled(self, scale);
 434 }
 435 template<typename Element, typename OtherElement>
 436 inline void vector3_scale(BasicVector3<Element>& self, const OtherElement& scale)
 437 {
 438   self.x() *= static_cast<Element>(scale);
 439   self.y() *= static_cast<Element>(scale);
 440   self.z() *= static_cast<Element>(scale);
 441 }
 442 template<typename Element, typename OtherElement>
 443 inline void operator*=(BasicVector3<Element>& self, const OtherElement& scale)
 444 {
 445   vector3_scale(self, scale);
 446 }
 447
 448 template<typename Element, typename OtherElement>
 449 inline BasicVector3<Element> vector3_divided(const BasicVector3<Element>& self, const BasicVector3<OtherElement>& other)
 450 {
 451   return BasicVector3<Element>(
 452     Element(self.x() / other.x()),
 453     Element(self.y() / other.y()),
 454     Element(self.z() / other.z())
 455   );
 456 }
 457 template<typename Element, typename OtherElement>
 458 inline BasicVector3<Element> operator/(const BasicVector3<Element>& self, const BasicVector3<OtherElement>& other)
 459 {
 460   return vector3_divided(self, other);
 461 }
 462 template<typename Element, typename OtherElement>
 463 inline void vector3_divide(BasicVector3<Element>& self, const BasicVector3<OtherElement>& other)
 464 {
 465   self.x() /= static_cast<Element>(other.x());
 466   self.y() /= static_cast<Element>(other.y());
 467   self.z() /= static_cast<Element>(other.z());
 468 }
 469 template<typename Element, typename OtherElement>
 470 inline void operator/=(BasicVector3<Element>& self, const BasicVector3<OtherElement>& other)
 471 {
 472   vector3_divide(self, other);
 473 }
 474
 475 template<typename Element, typename OtherElement>
 476 inline BasicVector3<Element> vector3_divided(const BasicVector3<Element>& self, const OtherElement& divisor)
 477 {
 478   return BasicVector3<Element>(
 479     Element(self.x() / divisor),
 480     Element(self.y() / divisor),
 481     Element(self.z() / divisor)
 482   );
 483 }
 484 template<typename Element, typename OtherElement>
 485 inline BasicVector3<Element> operator/(const BasicVector3<Element>& self, const OtherElement& divisor)
 486 {
 487   return vector3_divided(self, divisor);
 488 }
 489 template<typename Element, typename OtherElement>
 490 inline void vector3_divide(BasicVector3<Element>& self, const OtherElement& divisor)
 491 {
 492   self.x() /= static_cast<Element>(divisor);
 493   self.y() /= static_cast<Element>(divisor);
 494   self.z() /= static_cast<Element>(divisor);
 495 }
 496 template<typename Element, typename OtherElement>
 497 inline void operator/=(BasicVector3<Element>& self, const OtherElement& divisor)
 498 {
 499   vector3_divide(self, divisor);
 500 }
 501
 502 template<typename Element, typename OtherElement>
 503 inline double vector3_dot(const BasicVector3<Element>& self, const BasicVector3<OtherElement>& other)
 504 {
 505   return self.x() * other.x() + self.y() * other.y() + self.z() * other.z();
 506 }
 507
 508 template<typename Element>
 509 inline BasicVector3<Element> vector3_mid(const BasicVector3<Element>& begin, const BasicVector3<Element>& end)
 510 {
 511   return vector3_scaled(vector3_added(begin, end), 0.5);
 512 }
 513
 514 template<typename Element, typename OtherElement>
 515 inline BasicVector3<Element> vector3_cross(const BasicVector3<Element>& self, const BasicVector3<OtherElement>& other)
 516 {
 517   return BasicVector3<Element>(
 518     Element(self.y() * other.z() - self.z() * other.y()),
 519     Element(self.z() * other.x() - self.x() * other.z()),
 520     Element(self.x() * other.y() - self.y() * other.x())
 521   );
 522 }
 523
 524 template<typename Element>
 525 inline BasicVector3<Element> vector3_negated(const BasicVector3<Element>& self)
 526 {
 527   return BasicVector3<Element>(-self.x(), -self.y(), -self.z());
 528 }
 529 template<typename Element>
 530 inline BasicVector3<Element> operator-(const BasicVector3<Element>& self)
 531 {
 532   return vector3_negated(self);
 533 }
 534
 535 template<typename Element>
 536 inline void vector3_negate(BasicVector3<Element>& self)
 537 {
 538   self = vector3_negated(self);
 539 }
 540
 541 template<typename Element>
 542 inline double vector3_length_squared(const BasicVector3<Element>& self)
 543 {
 544   return vector3_dot(self, self);
 545 }
 546
 547 template<typename Element>
 548 inline double vector3_length(const BasicVector3<Element>& self)
 549 {
 550   return sqrt(vector3_length_squared(self));
 551 }
 552
 553 template<typename Element>
 554 inline Element float_divided(Element f, Element other)
 555 {
 556   //ASSERT_MESSAGE(other != 0, "float_divided: invalid divisor");
 557   return f / other;
 558 }
 559
 560 template<typename Element>
 561 inline BasicVector3<Element> vector3_normalised(const BasicVector3<Element>& self)
 562 {
 563   return vector3_scaled(self, float_divided(1.0, vector3_length(self)));
 564 }
 565
 566 template<typename Element>
 567 inline void vector3_normalise(BasicVector3<Element>& self)
 568 {
 569   self = vector3_normalised(self);
 570 }
 571
 572
 573 template<typename Element>
 574 inline BasicVector3<Element> vector3_snapped(const BasicVector3<Element>& self)
 575 {
 576   return BasicVector3<Element>(
 577     Element(float_to_integer(self.x())),
 578     Element(float_to_integer(self.y())),
 579     Element(float_to_integer(self.z()))
 580   );
 581 }
 582 template<typename Element>
 583 inline void vector3_snap(BasicVector3<Element>& self)
 584 {
 585   self = vector3_snapped(self);
 586 }
 587 template<typename Element, typename OtherElement>
 588 inline BasicVector3<Element> vector3_snapped(const BasicVector3<Element>& self, const OtherElement& snap)
 589 {
 590   return BasicVector3<Element>(
 591     Element(float_snapped(self.x(), snap)),
 592     Element(float_snapped(self.y(), snap)),
 593     Element(float_snapped(self.z(), snap))
 594   );
 595 }
 596 template<typename Element, typename OtherElement>
 597 inline void vector3_snap(BasicVector3<Element>& self, const OtherElement& snap)
 598 {
 599   self = vector3_snapped(self, snap);
 600 }
 601
 602 inline Vector3 vector3_for_spherical(double theta, double phi)
 603 {
 604   return Vector3(
 605     static_cast<float>(cos(theta) * cos(phi)),
 606     static_cast<float>(sin(theta) * cos(phi)),
 607     static_cast<float>(sin(phi))
 608   );
 609 }
 610
 611
 612
 613
 614 template<typename Element, typename OtherElement>
 615 inline bool vector4_equal(const BasicVector4<Element>& self, const BasicVector4<OtherElement>& other)
 616 {
 617   return self.x() == other.x() && self.y() == other.y() && self.z() == other.z() && self.w() == other.w();
 618 }
 619 template<typename Element, typename OtherElement>
 620 inline bool operator==(const BasicVector4<Element>& self, const BasicVector4<OtherElement>& other)
 621 {
 622   return vector4_equal(self, other);
 623 }
 624 template<typename Element, typename OtherElement>
 625 inline bool operator!=(const BasicVector4<Element>& self, const BasicVector4<OtherElement>& other)
 626 {
 627   return !vector4_equal(self, other);
 628 }
 629
 630 template<typename Element, typename OtherElement>
 631 inline bool vector4_equal_epsilon(const BasicVector4<Element>& self, const BasicVector4<OtherElement>& other, Element epsilon)
 632 {
 633   return float_equal_epsilon(self.x(), other.x(), epsilon)
 634     && float_equal_epsilon(self.y(), other.y(), epsilon)
 635     && float_equal_epsilon(self.z(), other.z(), epsilon)
 636     && float_equal_epsilon(self.w(), other.w(), epsilon);
 637 }
 638
 639 template<typename Element, typename OtherElement>
 640 inline BasicVector4<Element> vector4_added(const BasicVector4<Element>& self, const BasicVector4<OtherElement>& other)
 641 {
 642   return BasicVector4<Element>(
 643     float(self.x() + other.x()),
 644     float(self.y() + other.y()),
 645     float(self.z() + other.z()),
 646     float(self.w() + other.w())
 647   );
 648 }
 649 template<typename Element, typename OtherElement>
 650 inline BasicVector4<Element> operator+(const BasicVector4<Element>& self, const BasicVector4<OtherElement>& other)
 651 {
 652   return vector4_added(self, other);
 653 }
 654 template<typename Element, typename OtherElement>
 655 inline void vector4_add(BasicVector4<Element>& self, const BasicVector4<OtherElement>& other)
 656 {
 657   self.x() += static_cast<float>(other.x());
 658   self.y() += static_cast<float>(other.y());
 659   self.z() += static_cast<float>(other.z());
 660   self.w() += static_cast<float>(other.w());
 661 }
 662 template<typename Element, typename OtherElement>
 663 inline void operator+=(BasicVector4<Element>& self, const BasicVector4<OtherElement>& other)
 664 {
 665   vector4_add(self, other);
 666 }
 667
 668 template<typename Element, typename OtherElement>
 669 inline BasicVector4<Element> vector4_subtracted(const BasicVector4<Element>& self, const BasicVector4<OtherElement>& other)
 670 {
 671   return BasicVector4<Element>(
 672     float(self.x() - other.x()),
 673     float(self.y() - other.y()),
 674     float(self.z() - other.z()),
 675     float(self.w() - other.w())
 676   );
 677 }
 678 template<typename Element, typename OtherElement>
 679 inline BasicVector4<Element> operator-(const BasicVector4<Element>& self, const BasicVector4<OtherElement>& other)
 680 {
 681   return vector4_subtracted(self, other);
 682 }
 683 template<typename Element, typename OtherElement>
 684 inline void vector4_subtract(BasicVector4<Element>& self, const BasicVector4<OtherElement>& other)
 685 {
 686   self.x() -= static_cast<float>(other.x());
 687   self.y() -= static_cast<float>(other.y());
 688   self.z() -= static_cast<float>(other.z());
 689   self.w() -= static_cast<float>(other.w());
 690 }
 691 template<typename Element, typename OtherElement>
 692 inline void operator-=(BasicVector4<Element>& self, const BasicVector4<OtherElement>& other)
 693 {
 694   vector4_subtract(self, other);
 695 }
 696
 697 template<typename Element, typename OtherElement>
 698 inline BasicVector4<Element> vector4_scaled(const BasicVector4<Element>& self, const BasicVector4<OtherElement>& other)
 699 {
 700   return BasicVector4<Element>(
 701     float(self.x() * other.x()),
 702     float(self.y() * other.y()),
 703     float(self.z() * other.z()),
 704     float(self.w() * other.w())
 705   );
 706 }
 707 template<typename Element, typename OtherElement>
 708 inline BasicVector4<Element> operator*(const BasicVector4<Element>& self, const BasicVector4<OtherElement>& other)
 709 {
 710   return vector4_scaled(self, other);
 711 }
 712 template<typename Element, typename OtherElement>
 713 inline void vector4_scale(BasicVector4<Element>& self, const BasicVector4<OtherElement>& other)
 714 {
 715   self.x() *= static_cast<float>(other.x());
 716   self.y() *= static_cast<float>(other.y());
 717   self.z() *= static_cast<float>(other.z());
 718   self.w() *= static_cast<float>(other.w());
 719 }
 720 template<typename Element, typename OtherElement>
 721 inline void operator*=(BasicVector4<Element>& self, const BasicVector4<OtherElement>& other)
 722 {
 723   vector4_scale(self, other);
 724 }
 725
 726 template<typename Element, typename OtherElement>
 727 inline BasicVector4<Element> vector4_scaled(const BasicVector4<Element>& self, OtherElement scale)
 728 {
 729   return BasicVector4<Element>(
 730     float(self.x() * scale),
 731     float(self.y() * scale),
 732     float(self.z() * scale),
 733     float(self.w() * scale)
 734   );
 735 }
 736 template<typename Element, typename OtherElement>
 737 inline BasicVector4<Element> operator*(const BasicVector4<Element>& self, OtherElement scale)
 738 {
 739   return vector4_scaled(self, scale);
 740 }
 741 template<typename Element, typename OtherElement>
 742 inline void vector4_scale(BasicVector4<Element>& self, OtherElement scale)
 743 {
 744   self.x() *= static_cast<float>(scale);
 745   self.y() *= static_cast<float>(scale);
 746   self.z() *= static_cast<float>(scale);
 747   self.w() *= static_cast<float>(scale);
 748 }
 749 template<typename Element, typename OtherElement>
 750 inline void operator*=(BasicVector4<Element>& self, OtherElement scale)
 751 {
 752   vector4_scale(self, scale);
 753 }
 754
 755 template<typename Element, typename OtherElement>
 756 inline BasicVector4<Element> vector4_divided(const BasicVector4<Element>& self, OtherElement divisor)
 757 {
 758   return BasicVector4<Element>(
 759     float(self.x() / divisor),
 760     float(self.y() / divisor),
 761     float(self.z() / divisor),
 762     float(self.w() / divisor)
 763   );
 764 }
 765 template<typename Element, typename OtherElement>
 766 inline BasicVector4<Element> operator/(const BasicVector4<Element>& self, OtherElement divisor)
 767 {
 768   return vector4_divided(self, divisor);
 769 }
 770 template<typename Element, typename OtherElement>
 771 inline void vector4_divide(BasicVector4<Element>& self, OtherElement divisor)
 772 {
 773   self.x() /= divisor;
 774   self.y() /= divisor;
 775   self.z() /= divisor;
 776   self.w() /= divisor;
 777 }
 778 template<typename Element, typename OtherElement>
 779 inline void operator/=(BasicVector4<Element>& self, OtherElement divisor)
 780 {
 781   vector4_divide(self, divisor);
 782 }
 783
 784 template<typename Element, typename OtherElement>
 785 inline double vector4_dot(const BasicVector4<Element>& self, const BasicVector4<OtherElement>& other)
 786 {
 787   return self.x() * other.x() + self.y() * other.y() + self.z() * other.z() + self.w() * other.w();
 788 }
 789
 790 template<typename Element>
 791 inline BasicVector3<Element> vector4_projected(const BasicVector4<Element>& self)
 792 {
 793   return vector3_scaled(vector4_to_vector3(self), 1.0 / self[3]);
 794 }
 795
 796 #endif