float3.cpp

1 /* Copyright 2011 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 #ifdef MATH_ENABLE_STL_SUPPORT
19 #include <cassert>
20 #include <utility>
21 #endif
22 #include <stdlib.h>
23
24 #include "Math/float2.h"
25 #include "Math/float3.h"
26 #include "Math/float4.h"
27 #include "Math/float3x3.h"
28 #include "Geometry/Line.h"
29 #include "Geometry/Ray.h"
30 #include "Geometry/LineSegment.h"
31 #include "Geometry/Sphere.h"
32 #include "Geometry/AABB.h"
33 #include "Geometry/OBB.h"
34 #include "Geometry/Plane.h"
35 #include "Geometry/Triangle.h"
36 #include "Geometry/Capsule.h"
37 #include "Math/MathFunc.h"
38
39 MATH_BEGIN_NAMESPACE
40
41 using namespace std;
42
43 float3::float3(float x_, float y_, float z_)
44 :x(x_), y(y_), z(z_)
45 {
46 }
47
48 float3::float3(float scalar)
49 :x(scalar), y(scalar), z(scalar)
50 {
51 }
52
53 float3::float3(const float2 &xy, float z_)
54 :x(xy.x), y(xy.y), z(z_)
55 {
56 }
57
58 float3::float3(const float *data)
59 {
60         assume(data);
61 #ifndef MATH_ENABLE_INSECURE_OPTIMIZATIONS
62         if (!data)
63                 return;
64 #endif
65         x = data[0];
66         y = data[1];
67         z = data[2];
68 }
69
70 float *float3::ptr()
71 {
72         return &x;
73 }
74
75 const float *float3::ptr() const
76 {
77         return &x;
78 }
79
80 CONST_WIN32 float float3::At(int index) const
81 {
82         assume(index >= 0);
83         assume(index < Size);
84 #ifndef MATH_ENABLE_INSECURE_OPTIMIZATIONS
85         if (index < 0 || index >= Size)
86                 return FLOAT_NAN;
87 #endif
88         return ptr()[index];
89 }
90
91 float &float3::At(int index)
92 {
93         assume(index >= 0);
94         assume(index < Size);
95 #ifndef MATH_ENABLE_INSECURE_OPTIMIZATIONS
96         if (index < 0 || index >= Size)
97                 return ptr()[0];
98 #endif
99         return ptr()[index];
100 }
101
102 float2 float3::xx() const { return float2(x,x); }
103 float2 float3::xy() const { return float2(x,y); }
104 float2 float3::xz() const { return float2(x,z); }
105 float2 float3::yx() const { return float2(y,x); }
106 float2 float3::yy() const { return float2(y,y); }
107 float2 float3::yz() const { return float2(y,z); }
108 float2 float3::zx() const { return float2(z,x); }
109 float2 float3::zy() const { return float2(z,y); }
110 float2 float3::zz() const { return float2(z,z); }
111
112 float2 float3::Swizzled(int i, int j) const
113 {
114         return float2(At(i), At(j));
115 }
116
117 float3 float3::Swizzled(int i, int j, int k) const
118 {
119         return float3(At(i), At(j), At(k));
120 }
121
122 float4 float3::Swizzled(int i, int j, int k, int l) const
123 {
124         return float4(At(i), At(j), At(k), At(l));
125 }
126
127 float float3::LengthSq() const
128 {
129         return x*x + y*y + z*z;
130 }
131
132 float float3::Length() const
133 {
134         return sqrtf(LengthSq());
135 }
136
137 float float3::Normalize()
138 {
139         assume(IsFinite());
140         float lengthSq = LengthSq();
141         if (lengthSq > 1e-6f)
142         {
143                 float length = sqrtf(lengthSq);
144                 *this *= 1.f / length;
145                 return length;
146         }
147         else
148         {
149                 Set(1.f, 0.f, 0.f); // We will always produce a normalized vector.
150                 return 0; // But signal failure, so user knows we have generated an arbitrary normalization.
151         }
152 }
153
154 float3 float3::Normalized() const
155 {
156         float3 copy = *this;
157         float oldLength = copy.Normalize();
158         assume(oldLength > 0.f && "float3::Normalized() failed!");
159         return copy;
160 }
161
162 float float3::ScaleToLength(float newLength)
163 {
164         float length = LengthSq();
165         if (length < 1e-6f)
166         {
167                 Set(newLength, 0, 0); // Will always produce a vector of the requested length.
168                 return 0.f;
169         }
170
171         length = sqrtf(length);
172         float scalar = newLength / length;
173         x *= scalar;
174         y *= scalar;
175         z *= scalar;
176         return length;
177 }
178
179 float3 float3::ScaledToLength(float newLength) const
180 {
181         assume(!IsZero());
182
183         float3 v = *this;
184         v.ScaleToLength(newLength);
185         return v;
186 }
187
188 bool float3::IsNormalized(float epsilonSq) const
189 {
190         return fabs(LengthSq()-1.f) <= epsilonSq;
191 }
192
193 bool float3::IsZero(float epsilonSq) const
194 {
195         return fabs(LengthSq()) <= epsilonSq;
196 }
197
198 bool float3::IsFinite() const
199 {
200         return isfinite(x) && isfinite(y) && isfinite(z);
201 }
202
203 bool float3::IsPerpendicular(const float3 &other, float epsilon) const
204 {
205         return fabs(Dot(other)) <= epsilon;
206 }
207
208 #ifdef MATH_ENABLE_STL_SUPPORT
209 std::string float3::ToString() const
210 {
211         char str[256];
212         sprintf(str, "(%.3f, %.3f, %.3f)", x, y, z);
213         return std::string(str);
214 }
215
216 std::string float3::SerializeToString() const
217 {
218         char str[256];
219         sprintf(str, "%f %f %f", x, y, z);
220         return std::string(str);
221 }
222 #endif
223
224 float3 float3::FromString(const char *str)
225 {
226         assume(str);
227         if (!str)
228                 return float3::nan;
229         if (*str == '(')
230                 ++str;
231         float3 f;
232         f.x = (float)strtod(str, const_cast<char**>(&str));
233         if (*str == ',' || *str == ';')
234                 ++str;
235         f.y = (float)strtod(str, const_cast<char**>(&str));
236         if (*str == ',' || *str == ';')
237                 ++str;
238         f.z = (float)strtod(str, const_cast<char**>(&str));
239         return f;
240 }
241
242 float float3::SumOfElements() const
243 {
244         return x + y + z;
245 }
246
247 float float3::ProductOfElements() const
248 {
249         return x * y * z;
250 }
251
252 float float3::AverageOfElements() const
253 {
254         return (x + y + z) / 3.f;
255 }
256
257 float float3::MinElement() const
258 {
259         return MATH_NS::Min(MATH_NS::Min(x, y), z);
260 }
261
262 int float3::MinElementIndex() const
263 {
264         if (x <= y && x <= z)
265                 return 0;
266         else
267                 return (y <= z) ? 1 : 2;
268 }
269
270 float float3::MaxElement() const
271 {
272         return MATH_NS::Max(MATH_NS::Max(x, y), z);
273 }
274
275 int float3::MaxElementIndex() const
276 {
277         if (x >= y && x >= z)
278                 return 0;
279         else
280                 return (y >= z) ? 1 : 2;
281 }
282
283 float3 float3::Abs() const
284 {
285         return float3(fabs(x), fabs(y), fabs(z));
286 }
287
288 float3 float3::Neg() const
289 {
290         return float3(-x, -y, -z);
291 }
292
293 float3 float3::Recip() const
294 {
295         return float3(1.f/x, 1.f/y, 1.f/z);
296 }
297
298 float3 float3::Min(float ceil) const
299 {
300         return float3(MATH_NS::Min(x, ceil), MATH_NS::Min(y, ceil), MATH_NS::Min(z, ceil));
301 }
302
303 float3 float3::Min(const float3 &ceil) const
304 {
305         return float3(MATH_NS::Min(x, ceil.x), MATH_NS::Min(y, ceil.y), MATH_NS::Min(z, ceil.z));
306 }
307
308 float3 float3::Max(float floor) const
309 {
310         return float3(MATH_NS::Max(x, floor), MATH_NS::Max(y, floor), MATH_NS::Max(z, floor));
311 }
312
313 float3 float3::Max(const float3 &floor) const
314 {
315         return float3(MATH_NS::Max(x, floor.x), MATH_NS::Max(y, floor.y), MATH_NS::Max(z, floor.z));
316 }
317
318 float3 float3::Clamp(const float3 &floor, const float3 &ceil) const
319 {
320         return Min(ceil).Max(floor);
321 }
322
323 float3 float3::Clamp01() const
324 {
325         return Clamp(0.f, 1.f);
326 }
327
328 float3 float3::Clamp(float floor, float ceil) const
329 {
330         return Min(ceil).Max(floor);
331 }
332
333 float float3::DistanceSq(const float3 &rhs) const
334 {
335         float dx = x - rhs.x;
336         float dy = y - rhs.y;
337         float dz = z - rhs.z;
338         return dx*dx + dy*dy + dz*dz;
339 }
340
341 float float3::Distance(const float3 &rhs) const
342 {
343         return sqrtf(DistanceSq(rhs));
344 }
345
346 float float3::Distance(const Line &rhs) const { return rhs.Distance(*this); }
347 float float3::Distance(const Ray &rhs) const { return rhs.Distance(*this); }
348 float float3::Distance(const LineSegment &rhs) const { return rhs.Distance(*this); }
349 float float3::Distance(const Plane &rhs) const { return rhs.Distance(*this); }
350 float float3::Distance(const Triangle &rhs) const { return rhs.Distance(*this); }
351 float float3::Distance(const AABB &rhs) const { return rhs.Distance(*this); }
352 float float3::Distance(const OBB &rhs) const { return rhs.Distance(*this); }
353 float float3::Distance(const Sphere &rhs) const { return rhs.Distance(*this); }
354 float float3::Distance(const Capsule &rhs) const { return rhs.Distance(*this); }
355
356 float float3::Dot(const float3 &rhs) const
357 {
358         return x * rhs.x + y * rhs.y + z * rhs.z;
359 }
360
361 /** dst = A x B - The standard cross product:
362 \code
363                 |a cross b| = |a||b|sin(alpha)
364
365                 i               j               k               i               j               k               units (correspond to x,y,z)
366                 a               b               c               a               b               c               this vector
367                 d               e               f               d               e               f               vector v
368                 -cei    -afj    -bdk    bfi     cdj     aek     result
369
370                 x = bfi - cei = (bf-ce)i;
371                 y = cdj - afj = (cd-af)j;
372                 z - aek - bdk = (ae-bd)k;
373 \endcode
374
375 Cross product is anti-commutative, i.e. a x b == -b x a.
376 It distributes over addition, meaning that a x (b + c) == a x b + a x c,
377 and combines with scalar multiplication: (sa) x b == a x (sb).
378 i x j == -(j x i) == k,
379 (j x k) == -(k x j) == i,
380 (k x i) == -(i x k) == j. */
381 float3 float3::Cross(const float3 &rhs) const
382 {
383         float3 dst;
384         dst.x = y * rhs.z - z * rhs.y;
385         dst.y = z * rhs.x - x * rhs.z;
386         dst.z = x * rhs.y - y * rhs.x;
387         return dst;
388 }
389
390 float3x3 float3::OuterProduct(const float3 &rhs) const
391 {
392         const float3 &u = *this;
393         const float3 &v = rhs;
394         return float3x3(u[0]*v[0], u[0]*v[1], u[0]*v[2],
395                                         u[1]*v[0], u[1]*v[1], u[1]*v[2],
396                                         u[2]*v[0], u[2]*v[1], u[2]*v[2]);
397 }
398
399 float3 float3::Perpendicular(const float3 &hint, const float3 &hint2) const
400 {
401         assume(!this->IsZero());
402         assume(hint.IsNormalized());
403         assume(hint2.IsNormalized());
404         float3 v = this->Cross(hint);
405         float len = v.Normalize();
406         if (len == 0)
407                 return hint2;
408         else
409                 return v;
410 }
411
412 float3 float3::AnotherPerpendicular(const float3 &hint, const float3 &hint2) const
413 {
414         assume(!this->IsZero());
415         assume(hint.IsNormalized());
416         assume(hint2.IsNormalized());
417         float3 firstPerpendicular = Perpendicular(hint, hint2);
418         float3 v = this->Cross(firstPerpendicular);
419         return v.Normalized();
420 }
421
422 float float3::ScalarTripleProduct(const float3 &u, const float3 &v, const float3 &w)
423 {
424         return u.Cross(v).Dot(w);
425 }
426
427 float3 float3::Reflect(const float3 &normal) const
428 {
429         assume(normal.IsNormalized());
430         return 2.f * this->ProjectToNorm(normal) - *this;
431 }
432
433 /// Implementation from http://www.flipcode.com/archives/reflection_transmission.pdf .
434 float3 float3::Refract(const float3 &normal, float negativeSideRefractionIndex, float positiveSideRefractionIndex) const
435 {
436         // This code is duplicated in float2::Refract.
437         float n = negativeSideRefractionIndex / positiveSideRefractionIndex;
438         float cosI = this->Dot(normal);
439         float sinT2 = n*n*(1.f - cosI*cosI);
440         if (sinT2 > 1.f) // Total internal reflection occurs?
441                 return (-*this).Reflect(normal);
442         return n * *this - (n + Sqrt(1.f - sinT2)) * normal;
443 }
444
445 float3 float3::ProjectTo(const float3 &direction) const
446 {
447         assume(!direction.IsZero());
448         return direction * this->Dot(direction) / direction.LengthSq();
449 }
450
451 float3 float3::ProjectToNorm(const float3 &direction) const
452 {
453         assume(direction.IsNormalized());
454         return direction * this->Dot(direction);
455 }
456
457 float float3::AngleBetween(const float3 &other) const
458 {
459         return acos(Dot(other)) / sqrt(LengthSq() * other.LengthSq());
460 }
461
462 float float3::AngleBetweenNorm(const float3 &other) const
463 {
464         assume(this->IsNormalized());
465         assume(other.IsNormalized());
466         float cosa = Dot(other);
467         if (cosa >= 1.f)
468                 return 0.f;
469         else if (cosa <= -1.f)
470                 return pi;
471         else
472                 return acos(cosa);
473 }
474
475 void float3::Decompose(const float3 &direction, float3 &outParallel, float3 &outPerpendicular) const
476 {
477         assume(direction.IsNormalized());
478         outParallel = this->ProjectToNorm(direction);
479         outPerpendicular = *this - outParallel;
480 }
481
482 float3 float3::Lerp(const float3 &b, float t) const
483 {
484         assume(0.f <= t && t <= 1.f);
485         return (1.f - t) * *this + t * b;
486 }
487
488 float3 float3::Lerp(const float3 &a, const float3 &b, float t)
489 {
490         return a.Lerp(b, t);
491 }
492
493 void float3::Orthogonalize(const float3 &a, float3 &b)
494 {
495         if (!a.IsZero())
496                 b -= b.ProjectTo(a);
497 }
498
499 void float3::Orthogonalize(const float3 &a, float3 &b, float3 &c)
500 {
501         if (!a.IsZero())
502         {
503                 b -= b.ProjectTo(a);
504                 c -= c.ProjectTo(a);
505         }
506         if (!b.IsZero())
507                 c -= c.ProjectTo(b);
508 }
509
510 bool float3::AreOrthogonal(const float3 &a, const float3 &b, float epsilon)
511 {
512         return a.IsPerpendicular(b, epsilon);
513 }
514
515 bool float3::AreOrthogonal(const float3 &a, const float3 &b, const float3 &c, float epsilon)
516 {
517         return a.IsPerpendicular(b, epsilon) &&
518                    a.IsPerpendicular(c, epsilon) &&
519                    b.IsPerpendicular(c, epsilon);
520 }
521
522 void float3::Orthonormalize(float3 &a, float3 &b)
523 {
524         assume(!a.IsZero());
525         assume(!b.IsZero());
526         a.Normalize();
527         b -= b.ProjectToNorm(a);
528         b.Normalize();
529 }
530
531 void float3::Orthonormalize(float3 &a, float3 &b, float3 &c)
532 {
533         assume(!a.IsZero());
534         a.Normalize();
535         b -= b.ProjectToNorm(a);
536         assume(!b.IsZero());
537         b.Normalize();
538         c -= c.ProjectToNorm(a);
539         c -= c.ProjectToNorm(b);
540         assume(!c.IsZero());
541         c.Normalize();
542 }
543
544 bool float3::AreOrthonormal(const float3 &a, const float3 &b, float epsilon)
545 {
546         return a.IsPerpendicular(b, epsilon) && a.IsNormalized(epsilon*epsilon) && b.IsNormalized(epsilon*epsilon);
547 }
548
549 bool float3::AreOrthonormal(const float3 &a, const float3 &b, const float3 &c, float epsilon)
550 {
551         return a.IsPerpendicular(b, epsilon) &&
552                    a.IsPerpendicular(c, epsilon) &&
553                    b.IsPerpendicular(c, epsilon) &&
554                    a.IsNormalized(epsilon*epsilon) &&
555                    b.IsNormalized(epsilon*epsilon) &&
556                    c.IsNormalized(epsilon*epsilon);
557 }
558
559 float3 float3::FromScalar(float scalar)
560 {
561         return float3(scalar, scalar, scalar);
562 }
563
564 void float3::SetFromScalar(float scalar)
565 {
566         x = scalar;
567         y = scalar;
568         z = scalar;
569 }
570
571 void float3::Set(float x_, float y_, float z_)
572 {
573         x = x_;
574         y = y_;
575         z = z_;
576 }
577
578 bool float3::Equals(const float3 &other, float epsilon) const
579 {
580         return fabs(x - other.x) < epsilon &&
581                    fabs(y - other.y) < epsilon &&
582                    fabs(z - other.z) < epsilon;
583 }
584
585 bool float3::Equals(float x_, float y_, float z_, float epsilon) const
586 {
587         return fabs(x - x_) < epsilon &&
588                    fabs(y - y_) < epsilon &&
589                    fabs(z - z_) < epsilon;
590 }
591
592 float4 float3::ToPos4() const
593 {
594         return float4(*this, 1.f);
595 }
596
597 float4 float3::ToDir4() const
598 {
599         return float4(*this, 0.f);
600 }
601
602 float3 float3::RandomDir(LCG &lcg, float length)
603 {
604         return Sphere(float3(0,0,0), length).RandomPointOnSurface(lcg);
605 }
606
607 float3 float3::RandomSphere(LCG &lcg, const float3 &center, float radius)
608 {
609         return Sphere(center, radius).RandomPointInside(lcg);
610 }
611
612 float3 float3::RandomBox(LCG &lcg, float xmin, float xmax, float ymin, float ymax, float zmin, float zmax)
613 {
614         return RandomBox(lcg, float3(xmin, ymin, zmin), float3(xmax, ymax, zmax));
615 }
616
617 float3 float3::RandomBox(LCG &lcg, const float3 &minValues, const float3 &maxValues)
618 {
619         return AABB(minValues, maxValues).RandomPointInside(lcg);
620 }
621
622 float3 float3::operator +(const float3 &rhs) const
623 {
624         return float3(x + rhs.x, y + rhs.y, z + rhs.z);
625 }
626
627 float3 float3::operator -(const float3 &rhs) const
628 {
629         return float3(x - rhs.x, y - rhs.y, z - rhs.z);
630 }
631
632 float3 float3::operator -() const
633 {
634         return float3(-x, -y, -z);
635 }
636
637 float3 float3::operator *(float scalar) const
638 {
639         return float3(x * scalar, y * scalar, z * scalar);
640 }
641
642 float3 operator *(float scalar, const float3 &rhs)
643 {
644         return float3(scalar * rhs.x, scalar * rhs.y, scalar * rhs.z);
645 }
646
647 float3 float3::operator /(float scalar) const
648 {
649         float invScalar = 1.f / scalar;
650         return float3(x * invScalar, y * invScalar, z * invScalar);
651 }
652
653 float3 operator /(float scalar, const float3 &rhs)
654 {
655         return float3(scalar / rhs.x, scalar / rhs.y, scalar / rhs.z);
656 }
657
658 float3 &float3::operator +=(const float3 &rhs)
659 {
660         x += rhs.x;
661         y += rhs.y;
662         z += rhs.z;
663
664         return *this;
665 }
666
667 float3 &float3::operator -=(const float3 &rhs)
668 {
669         x -= rhs.x;
670         y -= rhs.y;
671         z -= rhs.z;
672
673         return *this;
674 }
675
676 float3 &float3::operator *=(float scalar)
677 {
678         x *= scalar;
679         y *= scalar;
680         z *= scalar;
681
682         return *this;
683 }
684
685 float3 float3::Add(float scalar) const
686 {
687         return float3(x + scalar, y + scalar, z + scalar);
688 }
689
690 float3 float3::Sub(float scalar) const
691 {
692         return float3(x - scalar, y - scalar, z - scalar);
693 }
694
695 float3 float3::SubLeft(float scalar) const
696 {
697         return float3(scalar - x, scalar - y, scalar - z);
698 }
699
700 float3 float3::Mul(const float3 &rhs) const
701 {
702         return float3(x * rhs.x, y * rhs.y, z * rhs.z);
703 }
704
705 float3 float3::Div(const float3 &rhs) const
706 {
707         return float3(x / rhs.x, y / rhs.y, z / rhs.z);
708 }
709
710 float3 float3::DivLeft(float scalar) const
711 {
712         return float3(scalar / x, scalar / y, scalar / z);
713 }
714
715 float3 &float3::operator /=(float scalar)
716 {
717         float invScalar = 1.f / scalar;
718         x *= invScalar;
719         y *= invScalar;
720         z *= invScalar;
721
722         return *this;
723 }
724
725 #ifdef MATH_ENABLE_STL_SUPPORT
726 std::ostream &operator <<(std::ostream &out, const float3 &rhs)
727 {
728         std::string str = rhs.ToString();
729         out << str;
730         return out;
731 }
732 #endif
733
734 const float3 float3::zero = float3(0, 0, 0);
735 const float3 float3::one = float3(1, 1, 1);
736 const float3 float3::unitX = float3(1, 0, 0);
737 const float3 float3::unitY = float3(0, 1, 0);
738 const float3 float3::unitZ = float3(0, 0, 1);
739 const float3 float3::nan = float3(FLOAT_NAN, FLOAT_NAN, FLOAT_NAN);
740 const float3 float3::inf = float3(FLOAT_INF, FLOAT_INF, FLOAT_INF);
741
742 MATH_END_NAMESPACE