float3.cpp

1 /* Copyright Jukka Jyl�nki
2
3    Licensed under the Apache License, Version 2.0 (the "License");
4    you may not use this file except in compliance with the License.
5    You may obtain a copy of the License at
6
7        http://www.apache.org/licenses/LICENSE-2.0
8
9    Unless required by applicable law or agreed to in writing, software
10    distributed under the License is distributed on an "AS IS" BASIS,
11    WITHOUT WARRANTIES OR CONDITIONS OF ANY KIND, either express or implied.
12    See the License for the specific language governing permissions and
13    limitations under the License. */
14
15 /** @file float3.cpp
16         @author Jukka Jyl�nki
17         @brief */
18 #include "float3.h"
19 #ifdef MATH_ENABLE_STL_SUPPORT
20 #include <iostream>
21 #include "myassert.h"
22 #include <utility>
23 #endif
24 #include <stdlib.h>
25
26 #include "float2.h"
27 #include "float4.h"
28 #include "float3x3.h"
29 #include "../Geometry/Line.h"
30 #include "../Geometry/Ray.h"
31 #include "../Geometry/LineSegment.h"
32 #include "../Geometry/Sphere.h"
33 #include "../Geometry/AABB.h"
34 #include "../Geometry/OBB.h"
35 #include "../Geometry/Plane.h"
36 #include "../Geometry/Triangle.h"
37 #include "../Geometry/Capsule.h"
38 #include "MathFunc.h"
39
40 #if defined(MATH_AUTOMATIC_SSE) && defined(MATH_SSE)
41 #include "float4_sse.h"
42 #endif
43
44 MATH_BEGIN_NAMESPACE
45
46 using namespace std;
47
48 float3::float3(float x_, float y_, float z_)
49 :x(x_), y(y_), z(z_)
50 {
51 }
52
53 float3::float3(float scalar)
54 :x(scalar), y(scalar), z(scalar)
55 {
56 }
57
58 float3::float3(const float2 &xy, float z_)
59 :x(xy.x), y(xy.y), z(z_)
60 {
61 }
62
63 float3::float3(const float *data)
64 {
65         assume(data);
66 #ifndef MATH_ENABLE_INSECURE_OPTIMIZATIONS
67         if (!data)
68                 return;
69 #endif
70         x = data[0];
71         y = data[1];
72         z = data[2];
73 }
74
75 CONST_WIN32 float float3::At(int index) const
76 {
77         assume(index >= 0);
78         assume(index < Size);
79 #ifndef MATH_ENABLE_INSECURE_OPTIMIZATIONS
80         if (index < 0 || index >= Size)
81                 return FLOAT_NAN;
82 #endif
83         return ptr()[index];
84 }
85
86 float &float3::At(int index)
87 {
88         assume(index >= 0);
89         assume(index < Size);
90 #ifndef MATH_ENABLE_INSECURE_OPTIMIZATIONS
91         if (index < 0 || index >= Size)
92                 return ptr()[0];
93 #endif
94         return ptr()[index];
95 }
96
97 float2 float3::xx() const { return float2(x,x); }
98 float2 float3::xy() const { return float2(x,y); }
99 float2 float3::xz() const { return float2(x,z); }
100 float2 float3::yx() const { return float2(y,x); }
101 float2 float3::yy() const { return float2(y,y); }
102 float2 float3::yz() const { return float2(y,z); }
103 float2 float3::zx() const { return float2(z,x); }
104 float2 float3::zy() const { return float2(z,y); }
105 float2 float3::zz() const { return float2(z,z); }
106
107 float2 float3::Swizzled(int i, int j) const
108 {
109         return float2(At(i), At(j));
110 }
111
112 float3 float3::Swizzled(int i, int j, int k) const
113 {
114         return float3(At(i), At(j), At(k));
115 }
116
117 float4 float3::Swizzled(int i, int j, int k, int l) const
118 {
119         return float4(At(i), At(j), At(k), At(l));
120 }
121
122 float float3::LengthSq() const
123 {
124         return x*x + y*y + z*z;
125 }
126
127 float float3::Length() const
128 {
129         return Sqrt(LengthSq());
130 }
131
132 float float3::Normalize()
133 {
134         assume(IsFinite());
135         float length = Length();
136         if (length > 1e-6f)
137         {
138                 *this *= 1.f / length;
139                 return length;
140         }
141         else
142         {
143                 Set(1.f, 0.f, 0.f); // We will always produce a normalized vector.
144                 return 0; // But signal failure, so user knows we have generated an arbitrary normalization.
145         }
146 }
147
148 float3 float3::Normalized() const
149 {
150         float3 copy = *this;
151         float oldLength = copy.Normalize();
152         assume(oldLength > 0.f && "float3::Normalized() failed!");
153         MARK_UNUSED(oldLength);
154         return copy;
155 }
156
157 float float3::ScaleToLength(float newLength)
158 {
159         float length = LengthSq();
160         if (length < 1e-6f)
161         {
162                 Set(newLength, 0, 0); // Will always produce a vector of the requested length.
163                 return 0.f;
164         }
165
166         length = Sqrt(length);
167         float scalar = newLength / length;
168         x *= scalar;
169         y *= scalar;
170         z *= scalar;
171         return length;
172 }
173
174 float3 float3::ScaledToLength(float newLength) const
175 {
176         assume(!IsZero());
177
178         float3 v = *this;
179         v.ScaleToLength(newLength);
180         return v;
181 }
182
183 bool float3::IsNormalized(float epsilonSq) const
184 {
185         return MATH_NS::Abs(LengthSq()-1.f) <= epsilonSq;
186 }
187
188 bool float3::IsZero(float epsilonSq) const
189 {
190         return LengthSq() <= epsilonSq;
191 }
192
193 bool float3::IsFinite() const
194 {
195         return MATH_NS::IsFinite(x) && MATH_NS::IsFinite(y) && MATH_NS::IsFinite(z);
196 }
197
198 bool float3::IsPerpendicular(const float3 &other, float epsilonSq) const
199 {
200         float dot = Dot(other);
201         return dot*dot <= epsilonSq * LengthSq() * other.LengthSq();
202 }
203
204 bool MUST_USE_RESULT float3::AreCollinear(const float3 &p1, const float3 &p2, const float3 &p3, float epsilonSq)
205 {
206         return (p2-p1).Cross(p3-p1).LengthSq() <= epsilonSq;
207 }
208
209 bool IsNeutralCLocale();
210
211 #ifdef MATH_ENABLE_STL_SUPPORT
212 std::string float3::ToString() const
213 {
214         char str[256];
215         sprintf(str, "(%.3f, %.3f, %.3f)", x, y, z);
216         return std::string(str);
217 }
218
219 std::string float3::SerializeToString() const
220 {
221         char str[256];
222         char *s = SerializeFloat(x, str); *s = ','; ++s;
223         s = SerializeFloat(y, s); *s = ','; ++s;
224         s = SerializeFloat(z, s);
225         assert(s+1 - str < 256);
226         MARK_UNUSED(s);
227         return str;
228 }
229
230 std::string float3::SerializeToCodeString() const
231 {
232         return "float3(" + SerializeToString() + ")";
233 }
234 #endif
235
236 float3 MUST_USE_RESULT float3::FromString(const char *str, const char **outEndStr)
237 {
238         assert(IsNeutralCLocale());
239         assume(str);
240         if (!str)
241                 return float3::nan;
242         MATH_SKIP_WORD(str, "float3");
243         MATH_SKIP_WORD(str, "(");
244         float3 f;
245         f.x = DeserializeFloat(str, &str);
246         f.y = DeserializeFloat(str, &str);
247         f.z = DeserializeFloat(str, &str);
248         if (*str == ')')
249                 ++str;
250         if (*str == ',')
251                 ++str;
252         if (outEndStr)
253                 *outEndStr = str;
254         return f;
255 }
256
257 int CountCommas(const char *start, const char *end)
258 {
259         int commas = 0;
260         while(start != end)
261                 if (*start++ == ',')
262                         ++commas;
263         return commas;
264 }
265
266 const char *FindNext(const char *str, char ch)
267 {
268         if (!str)
269                 return str;
270         while(*str)
271         {
272                 if (*str == ch)
273                         break;
274                 ++str;
275         }
276         return str;
277 }
278
279 vec PointVecFromString(const char *str, const char **outEndStr)
280 {
281         if (!str)
282                 return vec::nan;
283         if (MATH_NEXT_WORD_IS(str, "float3"))
284                 return POINT_VEC(float3::FromString(str, outEndStr));
285         if (MATH_NEXT_WORD_IS(str, "float4"))
286                 return FLOAT4_TO_POINT(float4::FromString(str, outEndStr));
287         while(*str == ' ')
288                 ++str;
289         if (*str == '(')
290         {
291                 const char *end = FindNext(str, ')');
292                 int numFields = CountCommas(str, end) + 1;
293                 assume1(numFields == 3 || numFields == 4, numFields);
294                 if (numFields == 4)
295                         return FLOAT4_TO_POINT(float4::FromString(str, outEndStr));
296         }
297         // Default to assuming that the shorter form of serialization was used and there is three floats.
298         return POINT_VEC(float3::FromString(str, outEndStr));
299 }
300
301 vec DirVecFromString(const char *str, const char **outEndStr)
302 {
303         if (!str)
304                 return vec::nan;
305         if (MATH_NEXT_WORD_IS(str, "float3"))
306                 return DIR_VEC(float3::FromString(str, outEndStr));
307         if (MATH_NEXT_WORD_IS(str, "float4"))
308                 return FLOAT4_TO_DIR(float4::FromString(str, outEndStr));
309         while(*str == ' ')
310                 ++str;
311         if (*str == '(')
312         {
313                 const char *end = FindNext(str, ')');
314                 int numFields = CountCommas(str, end) + 1;
315                 assume1(numFields == 3 || numFields == 4, numFields);
316                 if (numFields == 4)
317                         return FLOAT4_TO_DIR(float4::FromString(str, outEndStr));
318         }
319         // Default to assuming that the shorter form of serialization was used and there is three floats.
320         return DIR_VEC(float3::FromString(str, outEndStr));
321 }
322
323 float float3::SumOfElements() const
324 {
325         return x + y + z;
326 }
327
328 float float3::ProductOfElements() const
329 {
330         return x * y * z;
331 }
332
333 float float3::AverageOfElements() const
334 {
335         return (x + y + z) / 3.f;
336 }
337
338 float float3::MinElement() const
339 {
340         return MATH_NS::Min(MATH_NS::Min(x, y), z);
341 }
342
343 int float3::MinElementIndex() const
344 {
345         if (x <= y && x <= z)
346                 return 0;
347         else
348                 return (y <= z) ? 1 : 2;
349 }
350
351 float float3::MaxElement() const
352 {
353         return MATH_NS::Max(MATH_NS::Max(x, y), z);
354 }
355
356 int float3::MaxElementIndex() const
357 {
358         if (x >= y && x >= z)
359                 return 0;
360         else
361                 return (y >= z) ? 1 : 2;
362 }
363
364 float3 float3::Abs() const
365 {
366         return float3(MATH_NS::Abs(x), MATH_NS::Abs(y), MATH_NS::Abs(z));
367 }
368
369 float3 float3::Neg() const
370 {
371         return float3(-x, -y, -z);
372 }
373
374 float3 float3::Recip() const
375 {
376         return float3(1.f/x, 1.f/y, 1.f/z);
377 }
378
379 float3 float3::Min(float ceil) const
380 {
381         return float3(MATH_NS::Min(x, ceil), MATH_NS::Min(y, ceil), MATH_NS::Min(z, ceil));
382 }
383
384 float3 float3::Min(const float3 &ceil) const
385 {
386         return float3(MATH_NS::Min(x, ceil.x), MATH_NS::Min(y, ceil.y), MATH_NS::Min(z, ceil.z));
387 }
388
389 float3 float3::Max(float floor) const
390 {
391         return float3(MATH_NS::Max(x, floor), MATH_NS::Max(y, floor), MATH_NS::Max(z, floor));
392 }
393
394 float3 float3::Max(const float3 &floor) const
395 {
396         return float3(MATH_NS::Max(x, floor.x), MATH_NS::Max(y, floor.y), MATH_NS::Max(z, floor.z));
397 }
398
399 float3 float3::Clamp(const float3 &floor, const float3 &ceil) const
400 {
401         return Min(ceil).Max(floor);
402 }
403
404 float3 float3::Clamp01() const
405 {
406         return Clamp(0.f, 1.f);
407 }
408
409 float3 float3::Clamp(float floor, float ceil) const
410 {
411         return Min(ceil).Max(floor);
412 }
413
414 float3 float3::ClampLength(float maxLength) const
415 {
416         float lenSq = LengthSq();
417         if (lenSq > maxLength*maxLength)
418                 return *this * (maxLength / Sqrt(lenSq));
419         else
420                 return *this;
421 }
422
423 float3 float3::ClampLength(float minLength, float maxLength) const
424 {
425         float lenSq = LengthSq();
426         if (lenSq > maxLength*maxLength)
427                 return *this * (maxLength / Sqrt(lenSq));
428         else if (lenSq < minLength*minLength)
429                 return *this * (minLength / Sqrt(lenSq));
430         else
431                 return *this;
432 }
433
434 float float3::DistanceSq(const float3 &rhs) const
435 {
436         float dx = x - rhs.x;
437         float dy = y - rhs.y;
438         float dz = z - rhs.z;
439         return dx*dx + dy*dy + dz*dz;
440 }
441
442 float float3::Distance(const float3 &rhs) const
443 {
444         return Sqrt(DistanceSq(rhs));
445 }
446
447 float float3::Distance(const Line &rhs) const { return rhs.Distance(POINT_VEC(*this)); }
448 float float3::Distance(const Ray &rhs) const { return rhs.Distance(POINT_VEC(*this)); }
449 float float3::Distance(const LineSegment &rhs) const { return rhs.Distance(POINT_VEC(*this)); }
450 float float3::Distance(const Plane &rhs) const { return rhs.Distance(POINT_VEC(*this)); }
451 float float3::Distance(const Triangle &rhs) const { return rhs.Distance(POINT_VEC(*this)); }
452 float float3::Distance(const AABB &rhs) const { return rhs.Distance(POINT_VEC(*this)); }
453 float float3::Distance(const OBB &rhs) const { return rhs.Distance(POINT_VEC(*this)); }
454 float float3::Distance(const Sphere &rhs) const { return rhs.Distance(POINT_VEC(*this)); }
455 float float3::Distance(const Capsule &rhs) const { return rhs.Distance(POINT_VEC(*this)); }
456
457 float float3::Dot(const float3 &rhs) const
458 {
459         return x * rhs.x + y * rhs.y + z * rhs.z;
460 }
461
462 /** dst = A x B - The standard cross product:
463 \code
464                 |a cross b| = |a||b|sin(alpha)
465
466                 i               j               k               i               j               k               units (correspond to x,y,z)
467                 a               b               c               a               b               c               this vector
468                 d               e               f               d               e               f               vector v
469                 -cei    -afj    -bdk    bfi     cdj     aek     result
470
471                 x = bfi - cei = (bf-ce)i;
472                 y = cdj - afj = (cd-af)j;
473                 z - aek - bdk = (ae-bd)k;
474 \endcode
475
476 Cross product is anti-commutative, i.e. a x b == -b x a.
477 It distributes over addition, meaning that a x (b + c) == a x b + a x c,
478 and combines with scalar multiplication: (sa) x b == a x (sb).
479 i x j == -(j x i) == k,
480 (j x k) == -(k x j) == i,
481 (k x i) == -(i x k) == j. */
482 float3 float3::Cross(const float3 &rhs) const
483 {
484         return float3(y * rhs.z - z * rhs.y,
485                       z * rhs.x - x * rhs.z,
486                       x * rhs.y - y * rhs.x);
487 }
488
489 float3x3 float3::OuterProduct(const float3 &rhs) const
490 {
491         const float3 &u = *this;
492         const float3 &v = rhs;
493         return float3x3(u[0]*v[0], u[0]*v[1], u[0]*v[2],
494                                         u[1]*v[0], u[1]*v[1], u[1]*v[2],
495                                         u[2]*v[0], u[2]*v[1], u[2]*v[2]);
496 }
497
498 float3 float3::Perpendicular(const float3 &hint, const float3 &hint2) const
499 {
500         assume(!this->IsZero());
501         assume(hint.IsNormalized());
502         assume(hint2.IsNormalized());
503         float3 v = this->Cross(hint);
504         float len = v.Normalize();
505         if (len == 0)
506                 return hint2;
507         else
508                 return v;
509 }
510
511 float3 float3::AnotherPerpendicular(const float3 &hint, const float3 &hint2) const
512 {
513         assume(!this->IsZero());
514         assume(hint.IsNormalized());
515         assume(hint2.IsNormalized());
516         float3 firstPerpendicular = Perpendicular(hint, hint2);
517         float3 v = this->Cross(firstPerpendicular);
518         return v.Normalized();
519 }
520
521 void float3::PerpendicularBasis(float3 &outB, float3 &outC) const
522 {
523 #if defined(MATH_AUTOMATIC_SSE) && defined(MATH_SSE)
524         simd4f v = load_vec3(&x, 0.f);
525         simd4f out1, out2;
526         basis_ps(v, &out1, &out2);
527         store_vec3(&outB.x, out1);
528         store_vec3(&outC.x, out2);
529 #else
530         float3 a = this->Abs();
531         // Choose from (1,0,0), (0,1,0), and (0,0,1) the one that's most perpendicular to this vector.
532         float3 q;
533         if (a.x <= a.y)
534         {
535                 if (a.x <= a.z) q = float3(1,0,0);
536                 else q = float3(0,0,1);
537         }
538         else if (a.y <= a.z) q = float3(0,1,0);
539         else q = float3(0,0,1);
540
541         outB = this->Cross(q).Normalized();
542         outC = this->Cross(outB).Normalized();
543 #endif
544 }
545
546 float3 float3::RandomPerpendicular(LCG &rng) const
547 {
548         return Perpendicular(RandomDir(rng));
549 }
550
551 float MUST_USE_RESULT float3::ScalarTripleProduct(const float3 &u, const float3 &v, const float3 &w)
552 {
553         return u.Cross(v).Dot(w);
554 }
555
556 float3 float3::Reflect(const float3 &normal) const
557 {
558         assume2(normal.IsNormalized(), normal.SerializeToCodeString(), normal.Length());
559         return 2.f * this->ProjectToNorm(normal) - *this;
560 }
561
562 /// Implementation from http://www.flipcode.com/archives/reflection_transmission.pdf .
563 float3 float3::Refract(const float3 &normal, float negativeSideRefractionIndex, float positiveSideRefractionIndex) const
564 {
565         // This code is duplicated in float2::Refract.
566         float n = negativeSideRefractionIndex / positiveSideRefractionIndex;
567         float cosI = this->Dot(normal);
568         float sinT2 = n*n*(1.f - cosI*cosI);
569         if (sinT2 > 1.f) // Total internal reflection occurs?
570                 return (-*this).Reflect(normal);
571         return n * *this - (n + Sqrt(1.f - sinT2)) * normal;
572 }
573
574 float3 float3::ProjectTo(const float3 &direction) const
575 {
576         assume(!direction.IsZero());
577         return direction * this->Dot(direction) / direction.LengthSq();
578 }
579
580 float3 float3::ProjectToNorm(const float3 &direction) const
581 {
582         assume(direction.IsNormalized());
583         return direction * this->Dot(direction);
584 }
585
586 float float3::AngleBetween(const float3 &other) const
587 {
588         float cosa = Dot(other) / Sqrt(LengthSq() * other.LengthSq());
589         if (cosa >= 1.f)
590                 return 0.f;
591         else if (cosa <= -1.f)
592                 return pi;
593         else
594                 return acos(cosa);
595 }
596
597 float float3::AngleBetweenNorm(const float3 &other) const
598 {
599         assume(this->IsNormalized());
600         assume(other.IsNormalized());
601         float cosa = Dot(other);
602         if (cosa >= 1.f)
603                 return 0.f;
604         else if (cosa <= -1.f)
605                 return pi;
606         else
607                 return acos(cosa);
608 }
609
610 void float3::Decompose(const float3 &direction, float3 &outParallel, float3 &outPerpendicular) const
611 {
612         assume(direction.IsNormalized());
613         outParallel = this->ProjectToNorm(direction);
614         outPerpendicular = *this - outParallel;
615 }
616
617 float3 float3::Lerp(const float3 &b, float t) const
618 {
619         assume(0.f <= t && t <= 1.f);
620         return (1.f - t) * *this + t * b;
621 }
622
623 float3 MUST_USE_RESULT float3::Lerp(const float3 &a, const float3 &b, float t)
624 {
625         return a.Lerp(b, t);
626 }
627
628 void float3::Orthogonalize(const float3 &a, float3 &b)
629 {
630         if (!a.IsZero())
631                 b -= b.ProjectTo(a);
632 }
633
634 void float3::Orthogonalize(const float3 &a, float3 &b, float3 &c)
635 {
636         if (!a.IsZero())
637         {
638                 b -= b.ProjectTo(a);
639                 c -= c.ProjectTo(a);
640         }
641         if (!b.IsZero())
642                 c -= c.ProjectTo(b);
643 }
644
645 bool MUST_USE_RESULT float3::AreOrthogonal(const float3 &a, const float3 &b, float epsilon)
646 {
647         return a.IsPerpendicular(b, epsilon);
648 }
649
650 bool MUST_USE_RESULT float3::AreOrthogonal(const float3 &a, const float3 &b, const float3 &c, float epsilon)
651 {
652         return a.IsPerpendicular(b, epsilon) &&
653                a.IsPerpendicular(c, epsilon) &&
654                b.IsPerpendicular(c, epsilon);
655 }
656
657 void float3::Orthonormalize(float3 &a, float3 &b)
658 {
659         assume(!a.IsZero());
660         assume(!b.IsZero());
661         a.Normalize();
662         b -= b.ProjectToNorm(a);
663         b.Normalize();
664 }
665
666 void float3::Orthonormalize(float3 &a, float3 &b, float3 &c)
667 {
668         assume(!a.IsZero());
669         a.Normalize();
670         b -= b.ProjectToNorm(a);
671         assume(!b.IsZero());
672         b.Normalize();
673         c -= c.ProjectToNorm(a);
674         c -= c.ProjectToNorm(b);
675         assume(!c.IsZero());
676         c.Normalize();
677 }
678
679 bool MUST_USE_RESULT float3::AreOrthonormal(const float3 &a, const float3 &b, float epsilon)
680 {
681         return a.IsPerpendicular(b, epsilon) && a.IsNormalized(epsilon*epsilon) && b.IsNormalized(epsilon*epsilon);
682 }
683
684 bool MUST_USE_RESULT float3::AreOrthonormal(const float3 &a, const float3 &b, const float3 &c, float epsilon)
685 {
686         return a.IsPerpendicular(b, epsilon) &&
687                a.IsPerpendicular(c, epsilon) &&
688                b.IsPerpendicular(c, epsilon) &&
689                a.IsNormalized(epsilon*epsilon) &&
690                b.IsNormalized(epsilon*epsilon) &&
691                c.IsNormalized(epsilon*epsilon);
692 }
693
694 float3 MUST_USE_RESULT float3::FromScalar(float scalar)
695 {
696         return float3(scalar, scalar, scalar);
697 }
698
699 void float3::SetFromScalar(float scalar)
700 {
701         x = scalar;
702         y = scalar;
703         z = scalar;
704 }
705
706 void float3::Set(float x_, float y_, float z_)
707 {
708         x = x_;
709         y = y_;
710         z = z_;
711 }
712
713 void float3::SetFromSphericalCoordinates(float azimuth, float inclination, float radius)
714 {
715         float cx = Cos(inclination);
716         float sin, cos;
717         SinCos(azimuth, sin, cos);
718         x = cx * sin * radius;
719         y = -Sin(inclination) * radius;
720         z = cx * cos * radius;
721 }
722
723 float3 MUST_USE_RESULT float3::FromSphericalCoordinates(float azimuth, float inclination, float radius)
724 {
725         float3 v;
726         v.SetFromSphericalCoordinates(azimuth, inclination, radius);
727         return v;
728 }
729
730 void float3::SetFromSphericalCoordinates(float azimuth, float inclination)
731 {
732         float4 v, s, c;
733         v.x = inclination;
734         v.y = azimuth;
735         SinCos2(v, s, c);
736         x = c.x * s.y;
737         y = -s.x;
738         z = c.x * c.y;
739 }
740
741 float3 MUST_USE_RESULT float3::FromSphericalCoordinates(float azimuth, float inclination)
742 {
743         float3 v;
744         v.SetFromSphericalCoordinates(azimuth, inclination);
745         return v;
746 }
747
748 float3 float3::ToSphericalCoordinates() const
749 {
750         // R_y * R_x * (0,0,length) = (cosx*siny, -sinx, cosx*cosy).
751         float3 v = *this;
752         float len = v.Normalize();
753         if (len <= 1e-5f)
754                 return float3::zero;
755         float azimuth = atan2(v.x, v.z);
756         float inclination = asin(-v.y);
757         return float3(azimuth, inclination, len);
758 }
759
760 float2 float3::ToSphericalCoordinatesNormalized() const
761 {
762         assume(IsNormalized());
763         float azimuth = atan2(x, z);
764         float inclination = asin(-y);
765         return float2(azimuth, inclination);
766 }
767
768 bool float3::Equals(const float3 &other, float epsilon) const
769 {
770         return MATH_NS::Abs(x - other.x) < epsilon &&
771                MATH_NS::Abs(y - other.y) < epsilon &&
772                MATH_NS::Abs(z - other.z) < epsilon;
773 }
774
775 bool float3::Equals(float x_, float y_, float z_, float epsilon) const
776 {
777         return MATH_NS::Abs(x - x_) < epsilon &&
778                MATH_NS::Abs(y - y_) < epsilon &&
779                MATH_NS::Abs(z - z_) < epsilon;
780 }
781
782 bool float3::BitEquals(const float3 &other) const
783 {
784         return ReinterpretAsU32(x) == ReinterpretAsU32(other.x) &&
785                 ReinterpretAsU32(y) == ReinterpretAsU32(other.y) &&
786                 ReinterpretAsU32(z) == ReinterpretAsU32(other.z);
787 }
788
789 float4 float3::ToPos4() const
790 {
791         return float4(*this, 1.f);
792 }
793
794 float4 float3::ToDir4() const
795 {
796         return float4(*this, 0.f);
797 }
798
799 float3 MUST_USE_RESULT float3::RandomDir(LCG &lcg, float length)
800 {
801         return POINT_TO_FLOAT3(Sphere(POINT_VEC_SCALAR(0.f), length).RandomPointOnSurface(lcg));
802 }
803
804 float3 MUST_USE_RESULT float3::RandomSphere(LCG &lcg, const float3 &center, float radius)
805 {
806         return POINT_TO_FLOAT3(Sphere(POINT_VEC(center), radius).RandomPointInside(lcg));
807 }
808
809 float3 MUST_USE_RESULT float3::RandomBox(LCG &lcg, float xmin, float xmax, float ymin, float ymax, float zmin, float zmax)
810 {
811         return RandomBox(lcg, float3(xmin, ymin, zmin), float3(xmax, ymax, zmax));
812 }
813
814 float3 MUST_USE_RESULT float3::RandomBox(LCG &lcg, float minElem, float maxElem)
815 {
816         return RandomBox(lcg, float3(minElem, minElem, minElem), float3(maxElem, maxElem, maxElem));
817 }
818
819 float3 MUST_USE_RESULT float3::RandomBox(LCG &lcg, const float3 &minValues, const float3 &maxValues)
820 {
821         return POINT_TO_FLOAT3(AABB(POINT_VEC(minValues), POINT_VEC(maxValues)).RandomPointInside(lcg));
822 }
823
824 float3 float3::operator +(const float3 &rhs) const
825 {
826         return float3(x + rhs.x, y + rhs.y, z + rhs.z);
827 }
828
829 float3 float3::operator -(const float3 &rhs) const
830 {
831         return float3(x - rhs.x, y - rhs.y, z - rhs.z);
832 }
833
834 float3 float3::operator -() const
835 {
836         return float3(-x, -y, -z);
837 }
838
839 float3 float3::operator *(float scalar) const
840 {
841         return float3(x * scalar, y * scalar, z * scalar);
842 }
843
844 float3 operator *(float scalar, const float3 &rhs)
845 {
846         return float3(scalar * rhs.x, scalar * rhs.y, scalar * rhs.z);
847 }
848
849 float3 float3::operator /(float scalar) const
850 {
851         float invScalar = 1.f / scalar;
852         return float3(x * invScalar, y * invScalar, z * invScalar);
853 }
854
855 float3 &float3::operator +=(const float3 &rhs)
856 {
857         x += rhs.x;
858         y += rhs.y;
859         z += rhs.z;
860
861         return *this;
862 }
863
864 float3 &float3::operator -=(const float3 &rhs)
865 {
866         x -= rhs.x;
867         y -= rhs.y;
868         z -= rhs.z;
869
870         return *this;
871 }
872
873 float3 &float3::operator *=(float scalar)
874 {
875         x *= scalar;
876         y *= scalar;
877         z *= scalar;
878
879         return *this;
880 }
881
882 float3 float3::Add(float scalar) const
883 {
884         return float3(x + scalar, y + scalar, z + scalar);
885 }
886
887 float3 float3::Sub(float scalar) const
888 {
889         return float3(x - scalar, y - scalar, z - scalar);
890 }
891
892 float3 float3::SubLeft(float scalar) const
893 {
894         return float3(scalar - x, scalar - y, scalar - z);
895 }
896
897 float3 float3::Mul(const float3 &rhs) const
898 {
899         return float3(x * rhs.x, y * rhs.y, z * rhs.z);
900 }
901
902 float3 float3::Div(const float3 &rhs) const
903 {
904         return float3(x / rhs.x, y / rhs.y, z / rhs.z);
905 }
906
907 float3 float3::DivLeft(float scalar) const
908 {
909         return float3(scalar / x, scalar / y, scalar / z);
910 }
911
912 float3 &float3::operator /=(float scalar)
913 {
914         float invScalar = 1.f / scalar;
915         x *= invScalar;
916         y *= invScalar;
917         z *= invScalar;
918
919         return *this;
920 }
921
922 #ifdef MATH_ENABLE_STL_SUPPORT
923 std::ostream &operator <<(std::ostream &out, const float3 &rhs)
924 {
925         std::string str = rhs.ToString();
926         out << str;
927         return out;
928 }
929 #endif
930
931 const float3 float3::zero = float3(0, 0, 0);
932 const float3 float3::one = float3(1, 1, 1);
933 const float3 float3::unitX = float3(1, 0, 0);
934 const float3 float3::unitY = float3(0, 1, 0);
935 const float3 float3::unitZ = float3(0, 0, 1);
936 const float3 float3::nan = float3(FLOAT_NAN, FLOAT_NAN, FLOAT_NAN);
937 const float3 float3::inf = float3(FLOAT_INF, FLOAT_INF, FLOAT_INF);
938
939 MATH_END_NAMESPACE