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

Go back to previous page