1 #pragma once
2
3 #include <math.h>
4 #include <float.h>
5
6 #define LC_DTOR (static_cast<float>(M_PI / 180))
7 #define LC_RTOD (static_cast<float>(180 / M_PI))
8 #define LC_PI (static_cast<float>(M_PI))
9 #define LC_2PI (static_cast<float>(2 * M_PI))
10 #define LC_RGB_EPSILON (static_cast<float>(0.5f / 255.0f))
11
12 #define LC_RGB(r,g,b) LC_RGBA(r,g,b,255)
13 #define LC_RGBA(r,g,b,a) ((quint32)(((quint8) (r) | ((quint16) (g) << 8)) | (((quint32) (quint8) (b)) << 16) | (((quint32) (quint8) (a)) << 24)))
14 #define LC_RGBA_RED(rgba) ((quint8)(((rgba) >> 0) & 0xff))
15 #define LC_RGBA_GREEN(rgba) ((quint8)(((rgba) >> 8) & 0xff))
16 #define LC_RGBA_BLUE(rgba) ((quint8)(((rgba) >> 16) & 0xff))
17 #define LC_RGBA_ALPHA(rgba) ((quint8)(((rgba) >> 24) & 0xff))
18 #define LC_SRGB_TO_LINEAR(v) (powf(v, 2.2f))
19 #define LC_LINEAR_TO_SRGB(v) (powf(v, 1.0f / 2.2f))
20
lcRGBAFromQColor(const QColor & Color)21 inline quint32 lcRGBAFromQColor(const QColor& Color)
22 {
23 return LC_RGBA(Color.red(), Color.green(), Color.blue(), Color.alpha());
24 }
25
lcQColorFromRGBA(quint32 RGBA)26 inline QColor lcQColorFromRGBA(quint32 RGBA)
27 {
28 return QColor::fromRgb(LC_RGBA_RED(RGBA), LC_RGBA_GREEN(RGBA), LC_RGBA_BLUE(RGBA), LC_RGBA_ALPHA(RGBA));
29 }
30
31 template<typename T>
lcMin(const T & a,const T & b)32 inline T lcMin(const T& a, const T& b)
33 {
34 return a < b ? a : b;
35 }
36
37 template<typename T>
lcMax(const T & a,const T & b)38 inline T lcMax(const T& a, const T& b)
39 {
40 return a > b ? a : b;
41 }
42
43 template<typename T>
lcClamp(const T & Value,const T & Min,const T & Max)44 inline T lcClamp(const T& Value, const T& Min, const T& Max)
45 {
46 if (Value > Max)
47 return Max;
48 else if (Value < Min)
49 return Min;
50 else
51 return Value;
52 }
53
54 class lcVector2
55 {
56 public:
lcVector2()57 lcVector2()
58 {
59 }
60
lcVector2(const float _x,const float _y)61 constexpr lcVector2(const float _x, const float _y)
62 : x(_x), y(_y)
63 {
64 }
65
66 operator const float*() const
67 {
68 return (const float*)this;
69 }
70
71 operator float*()
72 {
73 return (float*)this;
74 }
75
76 const float& operator[](int i) const
77 {
78 return ((float*)this)[i];
79 }
80
81 float& operator[](int i)
82 {
83 return ((float*)this)[i];
84 }
85
IsNan()86 bool IsNan() const
87 {
88 return std::isnan(x) || std::isnan(y);
89 }
90
91 float x, y;
92 };
93
94 class lcVector3
95 {
96 public:
lcVector3()97 lcVector3()
98 {
99 }
100
lcVector3(const float _x,const float _y,const float _z)101 constexpr lcVector3(const float _x, const float _y, const float _z)
102 : x(_x), y(_y), z(_z)
103 {
104 }
105
106 explicit lcVector3(const lcVector4& v);
107
108 operator const float*() const
109 {
110 return (const float*)this;
111 }
112
113 operator float*()
114 {
115 return (float*)this;
116 }
117
118 const float& operator[](int i) const
119 {
120 return ((float*)this)[i];
121 }
122
123 float& operator[](int i)
124 {
125 return ((float*)this)[i];
126 }
127
IsNan()128 bool IsNan() const
129 {
130 return std::isnan(x) || std::isnan(y) || std::isnan(z);
131 }
132
133 void Normalize();
134 float Length() const;
135 float LengthSquared() const;
136
137 float x, y, z;
138 };
139
140 class lcVector4
141 {
142 public:
lcVector4()143 lcVector4()
144 {
145 }
146
lcVector4(const float _x,const float _y,const float _z,const float _w)147 constexpr lcVector4(const float _x, const float _y, const float _z, const float _w)
148 : x(_x), y(_y), z(_z), w(_w)
149 {
150 }
151
lcVector4(const lcVector3 & _xyz,const float _w)152 constexpr lcVector4(const lcVector3& _xyz, const float _w)
153 : x(_xyz.x), y(_xyz.y), z(_xyz.z), w(_w)
154 {
155 }
156
157 operator const float*() const
158 {
159 return (const float*)this;
160 }
161
162 operator float*()
163 {
164 return (float*)this;
165 }
166
167 const float& operator[](int i) const
168 {
169 return ((float*)this)[i];
170 }
171
172 float& operator[](int i)
173 {
174 return ((float*)this)[i];
175 }
176
IsNan()177 bool IsNan() const
178 {
179 return std::isnan(x) || std::isnan(y) || std::isnan(z) || std::isnan(w);
180 }
181
182 float x, y, z, w;
183 };
184
185 class lcMatrix33
186 {
187 public:
lcMatrix33()188 lcMatrix33()
189 {
190 }
191
lcMatrix33(const lcVector3 & _x,const lcVector3 & _y,const lcVector3 & _z)192 lcMatrix33(const lcVector3& _x, const lcVector3& _y, const lcVector3& _z)
193 {
194 r[0] = _x;
195 r[1] = _y;
196 r[2] = _z;
197 }
198
199 explicit lcMatrix33(const lcMatrix44& Matrix);
200
201 operator const float*() const
202 {
203 return (const float*)this;
204 }
205
206 operator float*()
207 {
208 return (float*)this;
209 }
210
211 const lcVector3& operator[](int i) const
212 {
213 return r[i];
214 }
215
216 lcVector3& operator[](int i)
217 {
218 return r[i];
219 }
220
221 void Orthonormalize();
222
223 lcVector3 r[3];
224 };
225
226 class lcMatrix44
227 {
228 public:
lcMatrix44()229 lcMatrix44()
230 {
231 }
232
lcMatrix44(const lcVector4 & _x,const lcVector4 & _y,const lcVector4 & _z,const lcVector4 & _w)233 lcMatrix44(const lcVector4& _x, const lcVector4& _y, const lcVector4& _z, const lcVector4& _w)
234 {
235 r[0] = _x;
236 r[1] = _y;
237 r[2] = _z;
238 r[3] = _w;
239 }
240
lcMatrix44(const lcMatrix33 & Rotation,const lcVector3 & Translation)241 lcMatrix44(const lcMatrix33& Rotation, const lcVector3& Translation)
242 {
243 r[0] = lcVector4(Rotation[0][0], Rotation[0][1], Rotation[0][2], 0.0f);
244 r[1] = lcVector4(Rotation[1][0], Rotation[1][1], Rotation[1][2], 0.0f);
245 r[2] = lcVector4(Rotation[2][0], Rotation[2][1], Rotation[2][2], 0.0f);
246 r[3] = lcVector4(Translation, 1.0f);
247 }
248
GetTranslation()249 lcVector3 GetTranslation() const
250 {
251 return lcVector3(r[3][0], r[3][1], r[3][2]);
252 }
253
SetTranslation(const lcVector3 & Translation)254 void SetTranslation(const lcVector3& Translation)
255 {
256 r[3] = lcVector4(Translation[0], Translation[1], Translation[2], 1.0f);
257 }
258
259 operator const float*() const
260 {
261 return (const float*)this;
262 }
263
264 operator float*()
265 {
266 return (float*)this;
267 }
268
269 const lcVector4& operator[](int i) const
270 {
271 return r[i];
272 }
273
274 lcVector4& operator[](int i)
275 {
276 return r[i];
277 }
278
279 float Determinant() const;
280
281 lcVector4 r[4];
282 };
283
lcVector3(const lcVector4 & v)284 inline lcVector3::lcVector3(const lcVector4& v)
285 : x(v.x), y(v.y), z(v.z)
286 {
287 }
288
289 inline lcVector3 operator+(const lcVector3& a, const lcVector3& b)
290 {
291 return lcVector3(a.x + b.x, a.y + b.y, a.z + b.z);
292 }
293
294 inline lcVector3 operator-(const lcVector3& a, const lcVector3& b)
295 {
296 return lcVector3(a.x - b.x, a.y - b.y, a.z - b.z);
297 }
298
299 inline lcVector3 operator*(const lcVector3& a, const lcVector3& b)
300 {
301 return lcVector3(a.x * b.x, a.y * b.y, a.z * b.z);
302 }
303
304 inline lcVector3 operator/(const lcVector3& a, const lcVector3& b)
305 {
306 return lcVector3(a.x / b.x, a.y / b.y, a.z / b.z);
307 }
308
309 inline lcVector3 operator*(const lcVector3& a, float b)
310 {
311 return lcVector3(a.x * b, a.y * b, a.z * b);
312 }
313
314 inline lcVector3 operator/(const lcVector3& a, float b)
315 {
316 return lcVector3(a.x / b, a.y / b, a.z / b);
317 }
318
319 inline lcVector3 operator*(float a, const lcVector3& b)
320 {
321 return lcVector3(b.x * a, b.y * a, b.z * a);
322 }
323
324 inline lcVector3 operator/(float a, const lcVector3& b)
325 {
326 return lcVector3(b.x / a, b.y / a, b.z / a);
327 }
328
329 inline lcVector3 operator-(const lcVector3& a)
330 {
331 return lcVector3(-a.x, -a.y, -a.z);
332 }
333
334 inline lcVector3& operator+=(lcVector3& a, const lcVector3& b)
335 {
336 a.x += b.x;
337 a.y += b.y;
338 a.z += b.z;
339
340 return a;
341 }
342
343 inline lcVector3& operator-=(lcVector3& a, const lcVector3& b)
344 {
345 a.x -= b.x;
346 a.y -= b.y;
347 a.z -= b.z;
348
349 return a;
350 }
351
352 inline lcVector3& operator*=(lcVector3& a, const lcVector3& b)
353 {
354 a.x *= b.x;
355 a.y *= b.y;
356 a.z *= b.z;
357
358 return a;
359 }
360
361 inline lcVector3& operator/=(lcVector3& a, const lcVector3& b)
362 {
363 a.x /= b.x;
364 a.y /= b.y;
365 a.z /= b.z;
366
367 return a;
368 }
369
370 inline lcVector3& operator+=(lcVector3& a, float b)
371 {
372 a.x += b;
373 a.y += b;
374 a.z += b;
375
376 return a;
377 }
378
379 inline lcVector3& operator*=(lcVector3& a, float b)
380 {
381 a.x *= b;
382 a.y *= b;
383 a.z *= b;
384
385 return a;
386 }
387
388 inline lcVector3& operator/=(lcVector3& a, float b)
389 {
390 a.x /= b;
391 a.y /= b;
392 a.z /= b;
393
394 return a;
395 }
396
397 inline bool operator==(const lcVector3& a, const lcVector3& b)
398 {
399 return a.x == b.x && a.y == b.y && a.z == b.z;
400 }
401
402 inline bool operator!=(const lcVector3& a, const lcVector3& b)
403 {
404 return a.x != b.x || a.y != b.y || a.z != b.z;
405 }
406
407 #ifndef QT_NO_DEBUG
408
409 inline QDebug operator<<(QDebug Debug, const lcVector2& v)
410 {
411 QDebugStateSaver Saver(Debug);
412 Debug.nospace() << '(' << v.x << ", " << v.y << ')';
413 return Debug;
414 }
415
416 inline QDebug operator<<(QDebug Debug, const lcVector3& v)
417 {
418 QDebugStateSaver Saver(Debug);
419 Debug.nospace() << '(' << v.x << ", " << v.y << ", " << v.z << ')';
420 return Debug;
421 }
422
423 inline QDebug operator<<(QDebug Debug, const lcVector4& v)
424 {
425 QDebugStateSaver Saver(Debug);
426 Debug.nospace() << '(' << v.x << ", " << v.y << ", " << v.z << ", " << v.w << ')';
427 return Debug;
428 }
429
430 inline QDebug operator<<(QDebug Debug, const lcMatrix33& m)
431 {
432 QDebugStateSaver Saver(Debug);
433 Debug.nospace() << '[' << m[0] << ", " << m[1] << ", " << m[2] << ']';
434 return Debug;
435 }
436
437 inline QDebug operator<<(QDebug Debug, const lcMatrix44& m)
438 {
439 QDebugStateSaver Saver(Debug);
440 Debug.nospace() << '[' << m[0] << ", " << m[1] << ", " << m[2] << ", " << m[3] << ']';
441 return Debug;
442 }
443
444 #endif
445
446 inline QDataStream& operator<<(QDataStream& Stream, const lcVector3& v)
447 {
448 Stream << v.x << v.y << v.z;
449 return Stream;
450 }
451
452 inline QDataStream& operator>>(QDataStream& Stream, lcVector3& v)
453 {
454 Stream >> v.x >> v.y >> v.z;
455 return Stream;
456 }
457
458 inline QDataStream& operator<<(QDataStream& Stream, const lcVector4& v)
459 {
460 Stream << v.x << v.y << v.z << v.w;
461 return Stream;
462 }
463
464 inline QDataStream& operator >> (QDataStream& Stream, lcVector4& v)
465 {
466 Stream >> v.x >> v.y >> v.z >> v.w;
467 return Stream;
468 }
469
Normalize()470 inline void lcVector3::Normalize()
471 {
472 const float InvLength = 1.0f / Length();
473
474 x *= InvLength;
475 y *= InvLength;
476 z *= InvLength;
477 }
478
Length()479 inline float lcVector3::Length() const
480 {
481 return sqrtf(x * x + y * y + z * z);
482 }
483
LengthSquared()484 inline float lcVector3::LengthSquared() const
485 {
486 return x * x + y * y + z * z;
487 }
488
lcLength(const lcVector3 & a)489 inline float lcLength(const lcVector3& a)
490 {
491 return a.Length();
492 }
493
lcLengthSquared(const lcVector3 & a)494 inline float lcLengthSquared(const lcVector3& a)
495 {
496 return a.LengthSquared();
497 }
498
lcNormalize(const lcVector3 & a)499 inline lcVector3 lcNormalize(const lcVector3& a)
500 {
501 lcVector3 Ret(a);
502 Ret.Normalize();
503 return Ret;
504 }
505
lcDot(const lcVector3 & a,const lcVector3 & b)506 inline float lcDot(const lcVector3& a, const lcVector3& b)
507 {
508 return a.x * b.x + a.y * b.y + a.z * b.z;
509 }
510
lcDot3(const lcVector4 & a,const lcVector3 & b)511 inline float lcDot3(const lcVector4& a, const lcVector3& b)
512 {
513 return a.x * b.x + a.y * b.y + a.z * b.z;
514 }
515
lcDot3(const lcVector3 & a,const lcVector4 & b)516 inline float lcDot3(const lcVector3& a, const lcVector4& b)
517 {
518 return a.x * b.x + a.y * b.y + a.z * b.z;
519 }
520
lcDot3(const lcVector4 & a,const lcVector4 & b)521 inline float lcDot3(const lcVector4& a, const lcVector4& b)
522 {
523 return a.x * b.x + a.y * b.y + a.z * b.z;
524 }
525
lcDot(const lcVector4 & a,const lcVector4 & b)526 inline float lcDot(const lcVector4& a, const lcVector4& b)
527 {
528 return a.x * b.x + a.y * b.y + a.z * b.z + a.w * b.w;
529 }
530
lcCross(const lcVector3 & a,const lcVector3 & b)531 inline lcVector3 lcCross(const lcVector3& a, const lcVector3& b)
532 {
533 return lcVector3(a.y * b.z - a.z * b.y, a.z * b.x - a.x * b.z, a.x * b.y - a.y * b.x);
534 }
535
536 template<>
537 inline lcVector3 lcMin<lcVector3>(const lcVector3& a, const lcVector3& b)
538 {
539 return lcVector3(a.x < b.x ? a.x : b.x, a.y < b.y ? a.y : b.y, a.z < b.z ? a.z : b.z);
540 }
541
542 template<>
543 inline lcVector3 lcMax<lcVector3>(const lcVector3& a, const lcVector3& b)
544 {
545 return lcVector3(a.x > b.x ? a.x : b.x, a.y > b.y ? a.y : b.y, a.z > b.z ? a.z : b.z);
546 }
547
548 inline lcVector4 operator+(const lcVector4& a, const lcVector4& b)
549 {
550 return lcVector4(a.x + b.x, a.y + b.y, a.z + b.z, a.w + b.w);
551 }
552
553 inline lcVector4 operator-(const lcVector4& a, const lcVector4& b)
554 {
555 return lcVector4(a.x - b.x, a.y - b.y, a.z - b.z, a.w - b.w);
556 }
557
558 inline lcVector4 operator*(const lcVector4& a, float f)
559 {
560 return lcVector4(a.x * f, a.y * f, a.z * f, a.w * f);
561 }
562
563 inline lcVector4 operator*(const lcVector4& a, const lcVector4& b)
564 {
565 return lcVector4(a.x * b.x, a.y * b.y, a.z * b.z, a.w * b.w);
566 }
567
568 inline lcVector4 operator/(const lcVector4& a, float f)
569 {
570 return lcVector4(a.x / f, a.y / f, a.z / f, a.w / f);
571 }
572
573 inline lcVector4 operator/(const lcVector4& a, const lcVector4& b)
574 {
575 return lcVector4(a.x / b.x, a.y / b.y, a.z / b.z, a.w / b.w);
576 }
577
578 inline lcVector4& operator+=(lcVector4& a, const lcVector4& b)
579 {
580 a.x += b.x;
581 a.y += b.y;
582 a.z += b.z;
583 a.w += b.w;
584
585 return a;
586 }
587
588 inline lcVector4& operator-=(lcVector4& a, const lcVector4& b)
589 {
590 a.x -= b.x;
591 a.y -= b.y;
592 a.z -= b.z;
593 a.w -= b.w;
594
595 return a;
596 }
597
598 inline lcVector4& operator*=(lcVector4& a, float b)
599 {
600 a.x *= b;
601 a.y *= b;
602 a.z *= b;
603 a.w *= b;
604
605 return a;
606 }
607
608 inline lcVector4& operator/=(lcVector4& a, float b)
609 {
610 a.x /= b;
611 a.y /= b;
612 a.z /= b;
613 a.w /= b;
614
615 return a;
616 }
617
lcPackNormal(const lcVector3 & Normal)618 inline quint32 lcPackNormal(const lcVector3& Normal)
619 {
620 quint32 Packed = 0;
621
622 Packed |= (((qint8)(Normal.x * 127.0f)) & 0xff) << 0;
623 Packed |= (((qint8)(Normal.y * 127.0f)) & 0xff) << 8;
624 Packed |= (((qint8)(Normal.z * 127.0f)) & 0xff) << 16;
625
626 return Packed;
627 }
628
lcUnpackNormal(quint32 Packed)629 inline lcVector3 lcUnpackNormal(quint32 Packed)
630 {
631 lcVector3 Normal;
632
633 Normal.x = (float)(qint8)((Packed >> 0) & 0xff) / 127.0f;
634 Normal.y = (float)(qint8)((Packed >> 8) & 0xff) / 127.0f;
635 Normal.z = (float)(qint8)((Packed >> 16) & 0xff) / 127.0f;
636
637 return Normal;
638 }
639
lcVector3LDrawToLeoCAD(const lcVector3 & Vector)640 inline lcVector3 lcVector3LDrawToLeoCAD(const lcVector3& Vector)
641 {
642 return lcVector3(Vector[0], Vector[2], -Vector[1]);
643 }
644
lcVector3FromColor(quint32 Color)645 inline lcVector3 lcVector3FromColor(quint32 Color)
646 {
647 lcVector3 v(LC_RGBA_RED(Color), LC_RGBA_GREEN(Color), LC_RGBA_BLUE(Color));
648 v /= 255.0f;
649 return v;
650 }
651
lcVector4FromColor(quint32 Color)652 inline lcVector4 lcVector4FromColor(quint32 Color)
653 {
654 lcVector4 v(LC_RGBA_RED(Color), LC_RGBA_GREEN(Color), LC_RGBA_BLUE(Color), LC_RGBA_ALPHA(Color));
655 v /= 255.0f;
656 return v;
657 }
658
lcColorFromVector3(const lcVector3 & Color)659 inline quint32 lcColorFromVector3(const lcVector3& Color)
660 {
661 return LC_RGB(roundf(Color[0] * 255), roundf(Color[1] * 255), roundf(Color[2] * 255));
662 }
663
lcLuminescence(const lcVector3 & Color)664 inline float lcLuminescence(const lcVector3& Color)
665 {
666 return 0.2126f * Color[0] + 0.7152f * Color[1] + 0.0722f * Color[2];
667 }
668
lcSRGBToLinear(const lcVector3 & Color)669 inline lcVector3 lcSRGBToLinear(const lcVector3& Color)
670 {
671 const float r = LC_SRGB_TO_LINEAR(Color[0]);
672 const float g = LC_SRGB_TO_LINEAR(Color[1]);
673 const float b = LC_SRGB_TO_LINEAR(Color[2]);
674
675 return lcVector3(r, g, b);
676 }
677
lcLinearToSRGB(const lcVector3 & Color)678 inline lcVector3 lcLinearToSRGB(const lcVector3& Color)
679 {
680 const float r = LC_LINEAR_TO_SRGB(Color[0]);
681 const float g = LC_LINEAR_TO_SRGB(Color[1]);
682 const float b = LC_LINEAR_TO_SRGB(Color[2]);
683
684 return lcVector3(r, g, b);
685 }
686
lcMul(const lcVector3 & a,const lcMatrix33 & b)687 inline lcVector3 lcMul(const lcVector3& a, const lcMatrix33& b)
688 {
689 return b.r[0] * a[0] + b.r[1] * a[1] + b.r[2] * a[2];
690 }
691
lcMul31(const lcVector3 & a,const lcMatrix44 & b)692 inline lcVector3 lcMul31(const lcVector3& a, const lcMatrix44& b)
693 {
694 lcVector4 v = b.r[0] * a[0] + b.r[1] * a[1] + b.r[2] * a[2] + b.r[3];
695
696 return lcVector3(v[0], v[1], v[2]);
697 }
698
lcMul31(const lcVector4 & a,const lcMatrix44 & b)699 inline lcVector3 lcMul31(const lcVector4& a, const lcMatrix44& b)
700 {
701 lcVector4 v = b.r[0] * a[0] + b.r[1] * a[1] + b.r[2] * a[2] + b.r[3];
702
703 return lcVector3(v[0], v[1], v[2]);
704 }
705
lcMul30(const lcVector3 & a,const lcMatrix44 & b)706 inline lcVector3 lcMul30(const lcVector3& a, const lcMatrix44& b)
707 {
708 lcVector4 v = b.r[0] * a[0] + b.r[1] * a[1] + b.r[2] * a[2];
709
710 return lcVector3(v[0], v[1], v[2]);
711 }
712
lcMul30(const lcVector4 & a,const lcMatrix44 & b)713 inline lcVector3 lcMul30(const lcVector4& a, const lcMatrix44& b)
714 {
715 lcVector4 v = b.r[0] * a[0] + b.r[1] * a[1] + b.r[2] * a[2];
716
717 return lcVector3(v[0], v[1], v[2]);
718 }
719
lcMul4(const lcVector4 & a,const lcMatrix44 & b)720 inline lcVector4 lcMul4(const lcVector4& a, const lcMatrix44& b)
721 {
722 return b.r[0] * a[0] + b.r[1] * a[1] + b.r[2] * a[2] + b.r[3] * a[3];
723 }
724
lcMul(const lcMatrix33 & a,const lcMatrix33 & b)725 inline lcMatrix33 lcMul(const lcMatrix33& a, const lcMatrix33& b)
726 {
727 const lcVector3 Col0(b.r[0][0], b.r[1][0], b.r[2][0]);
728 const lcVector3 Col1(b.r[0][1], b.r[1][1], b.r[2][1]);
729 const lcVector3 Col2(b.r[0][2], b.r[1][2], b.r[2][2]);
730
731 const lcVector3 Ret0(lcDot(a.r[0], Col0), lcDot(a.r[0], Col1), lcDot(a.r[0], Col2));
732 const lcVector3 Ret1(lcDot(a.r[1], Col0), lcDot(a.r[1], Col1), lcDot(a.r[1], Col2));
733 const lcVector3 Ret2(lcDot(a.r[2], Col0), lcDot(a.r[2], Col1), lcDot(a.r[2], Col2));
734
735 return lcMatrix33(Ret0, Ret1, Ret2);
736 }
737
lcMul(const lcMatrix44 & a,const lcMatrix44 & b)738 inline lcMatrix44 lcMul(const lcMatrix44& a, const lcMatrix44& b)
739 {
740 lcMatrix44 Result;
741
742 Result.r[0] = b.r[0] * a[0].x + b.r[1] * a[0].y + b.r[2] * a[0].z + b.r[3] * a[0].w;
743 Result.r[1] = b.r[0] * a[1].x + b.r[1] * a[1].y + b.r[2] * a[1].z + b.r[3] * a[1].w;
744 Result.r[2] = b.r[0] * a[2].x + b.r[1] * a[2].y + b.r[2] * a[2].z + b.r[3] * a[2].w;
745 Result.r[3] = b.r[0] * a[3].x + b.r[1] * a[3].y + b.r[2] * a[3].z + b.r[3] * a[3].w;
746
747 return Result;
748 }
749
lcMatrix33(const lcMatrix44 & Matrix)750 inline lcMatrix33::lcMatrix33(const lcMatrix44& Matrix)
751 {
752 r[0] = lcVector3(Matrix.r[0].x, Matrix.r[0].y, Matrix.r[0].z);
753 r[1] = lcVector3(Matrix.r[1].x, Matrix.r[1].y, Matrix.r[1].z);
754 r[2] = lcVector3(Matrix.r[2].x, Matrix.r[2].y, Matrix.r[2].z);
755 }
756
Orthonormalize()757 inline void lcMatrix33::Orthonormalize()
758 {
759 r[0] = lcNormalize(r[0]);
760 r[1] = lcNormalize(r[1] - lcDot(r[1], r[0]) * r[0]);
761 r[2] = r[2] - lcDot(r[2], r[0]) * r[0];
762 r[2] -= lcDot(r[2], r[1]) * r[1];
763 r[2] = lcNormalize(r[2]);
764 }
765
lcMatrix33Identity()766 inline lcMatrix33 lcMatrix33Identity()
767 {
768 lcMatrix33 m;
769
770 m.r[0] = lcVector3(1.0f, 0.0f, 0.0f);
771 m.r[1] = lcVector3(0.0f, 1.0f, 0.0f);
772 m.r[2] = lcVector3(0.0f, 0.0f, 1.0f);
773
774 return m;
775 }
776
lcMatrix33Scale(const lcVector3 & Scale)777 inline lcMatrix33 lcMatrix33Scale(const lcVector3& Scale)
778 {
779 lcMatrix33 m;
780
781 m.r[0] = lcVector3(Scale.x, 0.0f, 0.0f);
782 m.r[1] = lcVector3(0.0f, Scale.y, 0.0f);
783 m.r[2] = lcVector3(0.0f, 0.0f, Scale.z);
784
785 return m;
786 }
787
lcMatrix33RotationX(const float Radians)788 inline lcMatrix33 lcMatrix33RotationX(const float Radians)
789 {
790 float s, c;
791
792 s = sinf(Radians);
793 c = cosf(Radians);
794
795 lcMatrix33 m;
796
797 m.r[0] = lcVector3(1.0f, 0.0f, 0.0f);
798 m.r[1] = lcVector3(0.0f, c, s);
799 m.r[2] = lcVector3(0.0f, -s, c);
800
801 return m;
802 }
803
lcMatrix33RotationY(const float Radians)804 inline lcMatrix33 lcMatrix33RotationY(const float Radians)
805 {
806 float s, c;
807
808 s = sinf(Radians);
809 c = cosf(Radians);
810
811 lcMatrix33 m;
812
813 m.r[0] = lcVector3( c, 0.0f, -s);
814 m.r[1] = lcVector3(0.0f, 1.0f, 0.0f);
815 m.r[2] = lcVector3( s, 0.0f, c);
816
817 return m;
818 }
819
lcMatrix33RotationZ(const float Radians)820 inline lcMatrix33 lcMatrix33RotationZ(const float Radians)
821 {
822 float s, c;
823
824 s = sinf(Radians);
825 c = cosf(Radians);
826
827 lcMatrix33 m;
828
829 m.r[0] = lcVector3( c, s, 0.0f);
830 m.r[1] = lcVector3( -s, c, 0.0f);
831 m.r[2] = lcVector3(0.0f, 0.0f, 1.0f);
832
833 return m;
834 }
835
lcMatrix33FromAxisAngle(const lcVector3 & Axis,const float Radians)836 inline lcMatrix33 lcMatrix33FromAxisAngle(const lcVector3& Axis, const float Radians)
837 {
838 float s, c, mag, xx, yy, zz, xy, yz, zx, xs, ys, zs, one_c;
839
840 s = sinf(Radians);
841 c = cosf(Radians);
842 mag = Axis.Length();
843
844 if (mag == 0.0f)
845 return lcMatrix33Identity();
846
847 lcVector3 Normal = Axis * (1.0f / mag);
848
849 xx = Normal[0] * Normal[0];
850 yy = Normal[1] * Normal[1];
851 zz = Normal[2] * Normal[2];
852 xy = Normal[0] * Normal[1];
853 yz = Normal[1] * Normal[2];
854 zx = Normal[2] * Normal[0];
855 xs = Normal[0] * s;
856 ys = Normal[1] * s;
857 zs = Normal[2] * s;
858 one_c = 1.0f - c;
859
860 lcMatrix33 m;
861
862 m.r[0] = lcVector3((one_c * xx) + c, (one_c * xy) + zs, (one_c * zx) - ys);
863 m.r[1] = lcVector3((one_c * xy) - zs, (one_c * yy) + c, (one_c * yz) + xs);
864 m.r[2] = lcVector3((one_c * zx) + ys, (one_c * yz) - xs, (one_c * zz) + c);
865
866 return m;
867 }
868
lcMatrix33Transpose(const lcMatrix33 & m)869 inline lcMatrix33 lcMatrix33Transpose(const lcMatrix33& m)
870 {
871 lcMatrix33 t;
872
873 t.r[0] = lcVector3(m[0][0], m[1][0], m[2][0]);
874 t.r[1] = lcVector3(m[0][1], m[1][1], m[2][1]);
875 t.r[2] = lcVector3(m[0][2], m[1][2], m[2][2]);
876
877 return t;
878 }
879
lcMatrix33AffineInverse(const lcMatrix33 & m)880 inline lcMatrix33 lcMatrix33AffineInverse(const lcMatrix33& m)
881 {
882 lcMatrix33 Inv;
883
884 Inv.r[0] = lcVector3(m.r[0][0], m.r[1][0], m.r[2][0]);
885 Inv.r[1] = lcVector3(m.r[0][1], m.r[1][1], m.r[2][1]);
886 Inv.r[2] = lcVector3(m.r[0][2], m.r[1][2], m.r[2][2]);
887
888 return Inv;
889 }
890
lcMatrix33FromEulerAngles(const lcVector3 & Radians)891 inline lcMatrix33 lcMatrix33FromEulerAngles(const lcVector3& Radians)
892 {
893 float CosYaw, SinYaw, CosPitch, SinPitch, CosRoll, SinRoll;
894
895 CosRoll = cosf(Radians[0]);
896 SinRoll = sinf(Radians[0]);
897 CosPitch = cosf(Radians[1]);
898 SinPitch = sinf(Radians[1]);
899 CosYaw = cosf(Radians[2]);
900 SinYaw = sinf(Radians[2]);
901
902 lcMatrix33 m;
903
904 m.r[0] = lcVector3(CosYaw * CosPitch, SinYaw * CosPitch, -SinPitch);
905 m.r[1] = lcVector3(CosYaw * SinPitch * SinRoll - SinYaw * CosRoll, CosYaw * CosRoll + SinYaw * SinPitch * SinRoll, CosPitch * SinRoll);
906 m.r[2] = lcVector3(CosYaw * SinPitch * CosRoll + SinYaw * SinRoll, SinYaw * SinPitch * CosRoll - CosYaw * SinRoll, CosPitch * CosRoll);
907
908 return m;
909 }
910
lcMatrix33ToEulerAngles(const lcMatrix33 & RotMat)911 inline lcVector3 lcMatrix33ToEulerAngles(const lcMatrix33& RotMat)
912 {
913 float SinPitch, CosPitch, SinRoll, CosRoll, SinYaw, CosYaw;
914
915 SinPitch = -RotMat.r[0][2];
916 CosPitch = sqrtf(1 - SinPitch*SinPitch);
917
918 if (fabsf(CosPitch) > 0.0005f)
919 {
920 SinRoll = RotMat.r[1][2] / CosPitch;
921 CosRoll = RotMat.r[2][2] / CosPitch;
922 SinYaw = RotMat.r[0][1] / CosPitch;
923 CosYaw = RotMat.r[0][0] / CosPitch;
924 }
925 else
926 {
927 SinRoll = -RotMat.r[2][1];
928 CosRoll = RotMat.r[1][1];
929 SinYaw = 0.0f;
930 CosYaw = 1.0f;
931 }
932
933 lcVector3 Rot(atan2f(SinRoll, CosRoll), atan2f(SinPitch, CosPitch), atan2f(SinYaw, CosYaw));
934
935 if (Rot[0] < 0) Rot[0] += LC_2PI;
936 if (Rot[1] < 0) Rot[1] += LC_2PI;
937 if (Rot[2] < 0) Rot[2] += LC_2PI;
938
939 return Rot;
940 }
941
Determinant()942 inline float lcMatrix44::Determinant() const
943 {
944 return r[0][0] * r[1][1] * r[2][2] + r[0][1] * r[1][2] * r[2][0] +
945 r[0][2] * r[1][0] * r[2][1] - r[0][0] * r[1][2] * r[2][1] -
946 r[0][1] * r[1][0] * r[2][2] - r[0][2] * r[1][1] * r[2][0];
947 }
948
lcMatrix44Identity()949 inline lcMatrix44 lcMatrix44Identity()
950 {
951 lcMatrix44 m;
952
953 m.r[0] = lcVector4(1.0f, 0.0f, 0.0f, 0.0f);
954 m.r[1] = lcVector4(0.0f, 1.0f, 0.0f, 0.0f);
955 m.r[2] = lcVector4(0.0f, 0.0f, 1.0f, 0.0f);
956 m.r[3] = lcVector4(0.0f, 0.0f, 0.0f, 1.0f);
957
958 return m;
959 }
960
lcMatrix44Translation(const lcVector3 & Translation)961 inline lcMatrix44 lcMatrix44Translation(const lcVector3& Translation)
962 {
963 lcMatrix44 m;
964
965 m.r[0] = lcVector4(1.0f, 0.0f, 0.0f, 0.0f);
966 m.r[1] = lcVector4(0.0f, 1.0f, 0.0f, 0.0f);
967 m.r[2] = lcVector4(0.0f, 0.0f, 1.0f, 0.0f);
968 m.r[3] = lcVector4(Translation[0], Translation[1], Translation[2], 1.0f);
969
970 return m;
971 }
972
lcMatrix44RotationX(const float Radians)973 inline lcMatrix44 lcMatrix44RotationX(const float Radians)
974 {
975 float s, c;
976
977 s = sinf(Radians);
978 c = cosf(Radians);
979
980 lcMatrix44 m;
981
982 m.r[0] = lcVector4(1.0f, 0.0f, 0.0f, 0.0f);
983 m.r[1] = lcVector4(0.0f, c, s, 0.0f);
984 m.r[2] = lcVector4(0.0f, -s, c, 0.0f);
985 m.r[3] = lcVector4(0.0f, 0.0f, 0.0f, 1.0f);
986
987 return m;
988 }
989
lcMatrix44RotationY(const float Radians)990 inline lcMatrix44 lcMatrix44RotationY(const float Radians)
991 {
992 float s, c;
993
994 s = sinf(Radians);
995 c = cosf(Radians);
996
997 lcMatrix44 m;
998
999 m.r[0] = lcVector4( c, 0.0f, -s, 0.0f);
1000 m.r[1] = lcVector4(0.0f, 1.0f, 0.0f, 0.0f);
1001 m.r[2] = lcVector4( s, 0.0f, c, 0.0f);
1002 m.r[3] = lcVector4(0.0f, 0.0f, 0.0f, 1.0f);
1003
1004 return m;
1005 }
1006
lcMatrix44RotationZ(const float Radians)1007 inline lcMatrix44 lcMatrix44RotationZ(const float Radians)
1008 {
1009 float s, c;
1010
1011 s = sinf(Radians);
1012 c = cosf(Radians);
1013
1014 lcMatrix44 m;
1015
1016 m.r[0] = lcVector4( c, s, 0.0f, 0.0f);
1017 m.r[1] = lcVector4( -s, c, 0.0f, 0.0f);
1018 m.r[2] = lcVector4(0.0f, 0.0f, 1.0f, 0.0f);
1019 m.r[3] = lcVector4(0.0f, 0.0f, 0.0f, 1.0f);
1020
1021 return m;
1022 }
1023
lcMatrix44Scale(const lcVector3 & Scale)1024 inline lcMatrix44 lcMatrix44Scale(const lcVector3& Scale)
1025 {
1026 lcMatrix44 m;
1027
1028 m.r[0] = lcVector4(Scale.x, 0.0f, 0.0f, 0.0f);
1029 m.r[1] = lcVector4(0.0f, Scale.y, 0.0f, 0.0f);
1030 m.r[2] = lcVector4(0.0f, 0.0f, Scale.z, 0.0f);
1031 m.r[3] = lcVector4(0.0f, 0.0f, 0.0f, 1.0f);
1032
1033 return m;
1034 }
1035
lcMatrix44LookAt(const lcVector3 & Eye,const lcVector3 & Target,const lcVector3 & Up)1036 inline lcMatrix44 lcMatrix44LookAt(const lcVector3& Eye, const lcVector3& Target, const lcVector3& Up)
1037 {
1038 lcVector3 x, y, z;
1039
1040 z = lcNormalize(Eye - Target);
1041 x = lcNormalize(lcCross(Up, z));
1042 y = lcNormalize(lcCross(z, x));
1043
1044 lcMatrix44 m;
1045
1046 m.r[0] = lcVector4(x[0], y[0], z[0], 0.0f);
1047 m.r[1] = lcVector4(x[1], y[1], z[1], 0.0f);
1048 m.r[2] = lcVector4(x[2], y[2], z[2], 0.0f);
1049 m.r[3] = m.r[0] * -Eye[0] + m.r[1] * -Eye[1] + m.r[2] * -Eye[2];
1050 m.r[3][3] = 1.0f;
1051
1052 return m;
1053 }
1054
lcMatrix44Frustum(float Left,float Right,float Bottom,float Top,float Near,float Far)1055 inline lcMatrix44 lcMatrix44Frustum(float Left, float Right, float Bottom, float Top, float Near, float Far)
1056 {
1057 if ((Near <= 0.0f) || (Far <= 0.0f) || (Near == Far) || (Left == Right) || (Top == Bottom))
1058 return lcMatrix44Identity();
1059
1060 float x, y, a, b, c, d;
1061
1062 x = (2.0f * Near) / (Right - Left);
1063 y = (2.0f * Near) / (Top - Bottom);
1064 a = (Right + Left) / (Right - Left);
1065 b = (Top + Bottom) / (Top - Bottom);
1066 c = -(Far + Near) / (Far - Near);
1067 d = -(2.0f * Far * Near) / (Far - Near);
1068
1069 lcMatrix44 m;
1070
1071 m.r[0] = lcVector4(x, 0, 0, 0);
1072 m.r[1] = lcVector4(0, y, 0, 0);
1073 m.r[2] = lcVector4(a, b, c, -1);
1074 m.r[3] = lcVector4(0, 0, d, 0);
1075
1076 return m;
1077 }
1078
lcMatrix44Perspective(float FoVy,float Aspect,float Near,float Far)1079 inline lcMatrix44 lcMatrix44Perspective(float FoVy, float Aspect, float Near, float Far)
1080 {
1081 float Left, Right, Bottom, Top;
1082
1083 Top = Near * (float)tan(FoVy * LC_PI / 360.0f);
1084 Bottom = -Top;
1085
1086 Left = Bottom * Aspect;
1087 Right = Top * Aspect;
1088
1089 return lcMatrix44Frustum(Left, Right, Bottom, Top, Near, Far);
1090 }
1091
lcMatrix44Ortho(float Left,float Right,float Bottom,float Top,float Near,float Far)1092 inline lcMatrix44 lcMatrix44Ortho(float Left, float Right, float Bottom, float Top, float Near, float Far)
1093 {
1094 lcMatrix44 m;
1095
1096 m.r[0] = lcVector4(2.0f / (Right-Left), 0.0f, 0.0f, 0.0f),
1097 m.r[1] = lcVector4(0.0f, 2.0f / (Top-Bottom), 0.0f, 0.0f),
1098 m.r[2] = lcVector4(0.0f, 0.0f, -2.0f / (Far-Near), 0.0f),
1099 m.r[3] = lcVector4(-(Right+Left) / (Right-Left), -(Top+Bottom) / (Top-Bottom), -(Far+Near) / (Far-Near), 1.0f);
1100
1101 return m;
1102 }
1103
lcMatrix44FromAxisAngle(const lcVector3 & Axis,const float Radians)1104 inline lcMatrix44 lcMatrix44FromAxisAngle(const lcVector3& Axis, const float Radians)
1105 {
1106 float s, c, mag, xx, yy, zz, xy, yz, zx, xs, ys, zs, one_c;
1107
1108 s = sinf(Radians);
1109 c = cosf(Radians);
1110 mag = Axis.Length();
1111
1112 if (mag == 0.0f)
1113 return lcMatrix44Identity();
1114
1115 lcVector3 Normal = Axis * (1.0f / mag);
1116
1117 xx = Normal[0] * Normal[0];
1118 yy = Normal[1] * Normal[1];
1119 zz = Normal[2] * Normal[2];
1120 xy = Normal[0] * Normal[1];
1121 yz = Normal[1] * Normal[2];
1122 zx = Normal[2] * Normal[0];
1123 xs = Normal[0] * s;
1124 ys = Normal[1] * s;
1125 zs = Normal[2] * s;
1126 one_c = 1.0f - c;
1127
1128 lcMatrix44 m;
1129
1130 m.r[0] = lcVector4((one_c * xx) + c, (one_c * xy) + zs, (one_c * zx) - ys, 0.0f);
1131 m.r[1] = lcVector4((one_c * xy) - zs, (one_c * yy) + c, (one_c * yz) + xs, 0.0f);
1132 m.r[2] = lcVector4((one_c * zx) + ys, (one_c * yz) - xs, (one_c * zz) + c, 0.0f);
1133 m.r[3] = lcVector4(0.0f, 0.0f, 0.0f, 1.0f);
1134
1135 return m;
1136 }
1137
lcMatrix44ToAxisAngle(const lcMatrix44 & m)1138 inline lcVector4 lcMatrix44ToAxisAngle(const lcMatrix44& m)
1139 {
1140 lcVector3 Rows[3];
1141 Rows[0] = lcNormalize(lcVector3(m.r[0][0], m.r[0][1], m.r[0][2]));
1142 Rows[1] = lcNormalize(lcVector3(m.r[1][0], m.r[1][1], m.r[1][2]));
1143 Rows[2] = lcNormalize(lcVector3(m.r[2][0], m.r[2][1], m.r[2][2]));
1144
1145 if (m.Determinant() < 0.0f)
1146 Rows[0] *= -1.0f;
1147
1148 const float Trace = Rows[0][0] + Rows[1][1] + Rows[2][2];
1149 const float Cos = 0.5f * (Trace - 1.0f);
1150 lcVector4 rot;
1151
1152 rot[3] = acosf(lcClamp(Cos, -1.0f, 1.0f)); // in [0,PI]
1153
1154 if (rot[3] > 0.01f)
1155 {
1156 if (fabsf(LC_PI - rot[3]) > 0.01f)
1157 {
1158 rot[0] = Rows[1][2] - Rows[2][1];
1159 rot[1] = Rows[2][0] - Rows[0][2];
1160 rot[2] = Rows[0][1] - Rows[1][0];
1161
1162 float inv = 1.0f / sqrtf(rot[0]*rot[0] + rot[1]*rot[1] + rot[2]*rot[2]);
1163
1164 rot[0] *= inv;
1165 rot[1] *= inv;
1166 rot[2] *= inv;
1167 }
1168 else
1169 {
1170 // angle is PI
1171 float HalfInverse;
1172 if (Rows[0][0] >= Rows[1][1])
1173 {
1174 // r00 >= r11
1175 if (Rows[0][0] >= Rows[2][2])
1176 {
1177 // r00 is maximum diagonal term
1178 rot[0] = 0.5f * sqrtf(Rows[0][0] - Rows[1][1] - Rows[2][2] + 1.0f);
1179 HalfInverse = 0.5f / rot[0];
1180 rot[1] = HalfInverse * Rows[1][0];
1181 rot[2] = HalfInverse * Rows[2][0];
1182 }
1183 else
1184 {
1185 // r22 is maximum diagonal term
1186 rot[2] = 0.5f * sqrtf(Rows[2][2] - Rows[0][0] - Rows[1][1] + 1.0f);
1187 HalfInverse = 0.5f / rot[2];
1188 rot[0] = HalfInverse * Rows[2][0];
1189 rot[1] = HalfInverse * Rows[2][1];
1190 }
1191 }
1192 else
1193 {
1194 // r11 > r00
1195 if (Rows[1][1] >= Rows[2][2])
1196 {
1197 // r11 is maximum diagonal term
1198 rot[1] = 0.5f * sqrtf(Rows[1][1] - Rows[0][0] - Rows[2][2] + 1.0f);
1199 HalfInverse = 0.5f / rot[1];
1200 rot[0] = HalfInverse * Rows[1][0];
1201 rot[2] = HalfInverse * Rows[2][1];
1202 }
1203 else
1204 {
1205 // r22 is maximum diagonal term
1206 rot[2] = 0.5f * sqrtf(Rows[2][2] - Rows[0][0] - Rows[1][1] + 1.0f);
1207 HalfInverse = 0.5f / rot[2];
1208 rot[0] = HalfInverse * Rows[2][0];
1209 rot[1] = HalfInverse * Rows[2][1];
1210 }
1211 }
1212 }
1213 }
1214 else
1215 {
1216 // The angle is 0 and the matrix is the identity.
1217 rot[0] = 0.0f;
1218 rot[1] = 0.0f;
1219 rot[2] = 1.0f;
1220 }
1221
1222 return rot;
1223 }
1224
lcMatrix44FromEulerAngles(const lcVector3 & Radians)1225 inline lcMatrix44 lcMatrix44FromEulerAngles(const lcVector3& Radians)
1226 {
1227 float CosYaw, SinYaw, CosPitch, SinPitch, CosRoll, SinRoll;
1228
1229 CosRoll = cosf(Radians[0]);
1230 SinRoll = sinf(Radians[0]);
1231 CosPitch = cosf(Radians[1]);
1232 SinPitch = sinf(Radians[1]);
1233 CosYaw = cosf(Radians[2]);
1234 SinYaw = sinf(Radians[2]);
1235
1236 lcMatrix44 m;
1237
1238 m.r[0] = lcVector4(CosYaw * CosPitch, SinYaw * CosPitch, -SinPitch, 0.0f);
1239 m.r[1] = lcVector4(CosYaw * SinPitch * SinRoll - SinYaw * CosRoll, CosYaw * CosRoll + SinYaw * SinPitch * SinRoll, CosPitch * SinRoll, 0.0f);
1240 m.r[2] = lcVector4(CosYaw * SinPitch * CosRoll + SinYaw * SinRoll, SinYaw * SinPitch * CosRoll - CosYaw * SinRoll, CosPitch * CosRoll, 0.0f);
1241 m.r[3] = lcVector4(0.0f, 0.0f, 0.0f, 1.0f);
1242
1243 return m;
1244 }
1245
lcMatrix44ToEulerAngles(const lcMatrix44 & RotMat)1246 inline lcVector3 lcMatrix44ToEulerAngles(const lcMatrix44& RotMat)
1247 {
1248 float SinPitch, CosPitch, SinRoll, CosRoll, SinYaw, CosYaw;
1249
1250 SinPitch = -RotMat.r[0][2];
1251 CosPitch = sqrtf(1 - SinPitch*SinPitch);
1252
1253 if (fabsf(CosPitch) > 0.0005f)
1254 {
1255 SinRoll = RotMat.r[1][2] / CosPitch;
1256 CosRoll = RotMat.r[2][2] / CosPitch;
1257 SinYaw = RotMat.r[0][1] / CosPitch;
1258 CosYaw = RotMat.r[0][0] / CosPitch;
1259 }
1260 else
1261 {
1262 SinRoll = -RotMat.r[2][1];
1263 CosRoll = RotMat.r[1][1];
1264 SinYaw = 0.0f;
1265 CosYaw = 1.0f;
1266 }
1267
1268 lcVector3 Rot(atan2f(SinRoll, CosRoll), atan2f(SinPitch, CosPitch), atan2f(SinYaw, CosYaw));
1269
1270 if (Rot[0] < 0) Rot[0] += LC_2PI;
1271 if (Rot[1] < 0) Rot[1] += LC_2PI;
1272 if (Rot[2] < 0) Rot[2] += LC_2PI;
1273
1274 return Rot;
1275 }
1276
lcMatrix44Transpose(const lcMatrix44 & m)1277 inline lcMatrix44 lcMatrix44Transpose(const lcMatrix44& m)
1278 {
1279 lcMatrix44 t;
1280
1281 t.r[0] = lcVector4(m[0][0], m[1][0], m[2][0], m[3][0]);
1282 t.r[1] = lcVector4(m[0][1], m[1][1], m[2][1], m[3][1]);
1283 t.r[2] = lcVector4(m[0][2], m[1][2], m[2][2], m[3][2]);
1284 t.r[3] = lcVector4(m[0][3], m[1][3], m[2][3], m[3][3]);
1285
1286 return t;
1287 }
1288
lcMatrix44AffineInverse(const lcMatrix44 & m)1289 inline lcMatrix44 lcMatrix44AffineInverse(const lcMatrix44& m)
1290 {
1291 lcMatrix44 Inv;
1292
1293 Inv.r[0] = lcVector4(m.r[0][0], m.r[1][0], m.r[2][0], m.r[0][3]);
1294 Inv.r[1] = lcVector4(m.r[0][1], m.r[1][1], m.r[2][1], m.r[1][3]);
1295 Inv.r[2] = lcVector4(m.r[0][2], m.r[1][2], m.r[2][2], m.r[2][3]);
1296
1297 lcVector3 Trans = -lcMul30(m.r[3], Inv);
1298 Inv.r[3] = lcVector4(Trans[0], Trans[1], Trans[2], 1.0f);
1299
1300 return Inv;
1301 }
1302
1303 // Inverse code from the GLU library.
lcMatrix44Inverse(const lcMatrix44 & m)1304 inline lcMatrix44 lcMatrix44Inverse(const lcMatrix44& m)
1305 {
1306 #define SWAP_ROWS(a, b) { float *_tmp = a; (a)=(b); (b)=_tmp; }
1307 #define MAT(m,col,row) m.r[row][col]
1308
1309 float wtmp[4][8];
1310 float m0, m1, m2, m3, s;
1311 float *r0, *r1, *r2, *r3;
1312
1313 r0 = wtmp[0], r1 = wtmp[1], r2 = wtmp[2], r3 = wtmp[3];
1314
1315 r0[0] = MAT(m,0,0), r0[1] = MAT(m,0,1),
1316 r0[2] = MAT(m,0,2), r0[3] = MAT(m,0,3),
1317 r0[4] = 1.0, r0[5] = r0[6] = r0[7] = 0.0,
1318
1319 r1[0] = MAT(m,1,0), r1[1] = MAT(m,1,1),
1320 r1[2] = MAT(m,1,2), r1[3] = MAT(m,1,3),
1321 r1[5] = 1.0, r1[4] = r1[6] = r1[7] = 0.0,
1322
1323 r2[0] = MAT(m,2,0), r2[1] = MAT(m,2,1),
1324 r2[2] = MAT(m,2,2), r2[3] = MAT(m,2,3),
1325 r2[6] = 1.0, r2[4] = r2[5] = r2[7] = 0.0,
1326
1327 r3[0] = MAT(m,3,0), r3[1] = MAT(m,3,1),
1328 r3[2] = MAT(m,3,2), r3[3] = MAT(m,3,3),
1329 r3[7] = 1.0, r3[4] = r3[5] = r3[6] = 0.0;
1330
1331 // choose pivot - or die
1332 if (fabs(r3[0])>fabs(r2[0])) SWAP_ROWS(r3, r2);
1333 if (fabs(r2[0])>fabs(r1[0])) SWAP_ROWS(r2, r1);
1334 if (fabs(r1[0])>fabs(r0[0])) SWAP_ROWS(r1, r0);
1335 // if (0.0 == r0[0]) return GL_FALSE;
1336
1337 // eliminate first variable
1338 m1 = r1[0]/r0[0]; m2 = r2[0]/r0[0]; m3 = r3[0]/r0[0];
1339 s = r0[1]; r1[1] -= m1 * s; r2[1] -= m2 * s; r3[1] -= m3 * s;
1340 s = r0[2]; r1[2] -= m1 * s; r2[2] -= m2 * s; r3[2] -= m3 * s;
1341 s = r0[3]; r1[3] -= m1 * s; r2[3] -= m2 * s; r3[3] -= m3 * s;
1342 s = r0[4];
1343 if (s != 0.0) { r1[4] -= m1 * s; r2[4] -= m2 * s; r3[4] -= m3 * s; }
1344 s = r0[5];
1345 if (s != 0.0) { r1[5] -= m1 * s; r2[5] -= m2 * s; r3[5] -= m3 * s; }
1346 s = r0[6];
1347 if (s != 0.0) { r1[6] -= m1 * s; r2[6] -= m2 * s; r3[6] -= m3 * s; }
1348 s = r0[7];
1349 if (s != 0.0) { r1[7] -= m1 * s; r2[7] -= m2 * s; r3[7] -= m3 * s; }
1350
1351 // choose pivot - or die
1352 if (fabs(r3[1])>fabs(r2[1])) SWAP_ROWS(r3, r2);
1353 if (fabs(r2[1])>fabs(r1[1])) SWAP_ROWS(r2, r1);
1354 // if (0.0 == r1[1]) return GL_FALSE;
1355
1356 // eliminate second variable
1357 m2 = r2[1]/r1[1]; m3 = r3[1]/r1[1];
1358 r2[2] -= m2 * r1[2]; r3[2] -= m3 * r1[2];
1359 r2[3] -= m2 * r1[3]; r3[3] -= m3 * r1[3];
1360 s = r1[4]; if (0.0 != s) { r2[4] -= m2 * s; r3[4] -= m3 * s; }
1361 s = r1[5]; if (0.0 != s) { r2[5] -= m2 * s; r3[5] -= m3 * s; }
1362 s = r1[6]; if (0.0 != s) { r2[6] -= m2 * s; r3[6] -= m3 * s; }
1363 s = r1[7]; if (0.0 != s) { r2[7] -= m2 * s; r3[7] -= m3 * s; }
1364
1365 // choose pivot - or die
1366 if (fabs(r3[2])>fabs(r2[2])) SWAP_ROWS(r3, r2);
1367 // if (0.0 == r2[2]) return GL_FALSE;
1368
1369 // eliminate third variable
1370 m3 = r3[2]/r2[2];
1371 r3[3] -= m3 * r2[3], r3[4] -= m3 * r2[4],
1372 r3[5] -= m3 * r2[5], r3[6] -= m3 * r2[6],
1373 r3[7] -= m3 * r2[7];
1374
1375 // last check
1376 // if (0.0 == r3[3]) return GL_FALSE;
1377
1378 s = 1.0f/r3[3]; // now back substitute row 3
1379 r3[4] *= s; r3[5] *= s; r3[6] *= s; r3[7] *= s;
1380
1381 m2 = r2[3]; // now back substitute row 2
1382 s = 1.0f/r2[2];
1383 r2[4] = s * (r2[4] - r3[4] * m2), r2[5] = s * (r2[5] - r3[5] * m2),
1384 r2[6] = s * (r2[6] - r3[6] * m2), r2[7] = s * (r2[7] - r3[7] * m2);
1385 m1 = r1[3];
1386 r1[4] -= r3[4] * m1, r1[5] -= r3[5] * m1,
1387 r1[6] -= r3[6] * m1, r1[7] -= r3[7] * m1;
1388 m0 = r0[3];
1389 r0[4] -= r3[4] * m0, r0[5] -= r3[5] * m0,
1390 r0[6] -= r3[6] * m0, r0[7] -= r3[7] * m0;
1391
1392 m1 = r1[2]; // now back substitute row 1
1393 s = 1.0f/r1[1];
1394 r1[4] = s * (r1[4] - r2[4] * m1), r1[5] = s * (r1[5] - r2[5] * m1),
1395 r1[6] = s * (r1[6] - r2[6] * m1), r1[7] = s * (r1[7] - r2[7] * m1);
1396 m0 = r0[2];
1397 r0[4] -= r2[4] * m0, r0[5] -= r2[5] * m0,
1398 r0[6] -= r2[6] * m0, r0[7] -= r2[7] * m0;
1399
1400 m0 = r0[1]; // now back substitute row 0
1401 s = 1.0f/r0[0];
1402 r0[4] = s * (r0[4] - r1[4] * m0), r0[5] = s * (r0[5] - r1[5] * m0),
1403 r0[6] = s * (r0[6] - r1[6] * m0), r0[7] = s * (r0[7] - r1[7] * m0);
1404
1405 const lcVector4 Row0(r0[4], r1[4], r2[4], r3[4]);
1406 const lcVector4 Row1(r0[5], r1[5], r2[5], r3[5]);
1407 const lcVector4 Row2(r0[6], r1[6], r2[6], r3[6]);
1408 const lcVector4 Row3(r0[7], r1[7], r2[7], r3[7]);
1409
1410 lcMatrix44 out(Row0, Row1, Row2, Row3);
1411
1412 return out;
1413
1414 #undef MAT
1415 #undef SWAP_ROWS
1416 }
1417
lcMatrix44LeoCADToLDraw(const lcMatrix44 & Matrix)1418 inline lcMatrix44 lcMatrix44LeoCADToLDraw(const lcMatrix44& Matrix)
1419 {
1420 lcMatrix44 m;
1421
1422 m.r[0] = lcVector4(Matrix[0][0], -Matrix[2][0], Matrix[1][0], 0.0f);
1423 m.r[1] = lcVector4(-Matrix[0][2], Matrix[2][2], -Matrix[1][2], 0.0f);
1424 m.r[2] = lcVector4(Matrix[0][1], -Matrix[2][1], Matrix[1][1], 0.0f);
1425 m.r[3] = lcVector4(Matrix[3][0], -Matrix[3][2], Matrix[3][1], 1.0f);
1426
1427 return m;
1428 }
1429
lcMatrix44LDrawToLeoCAD(const lcMatrix44 & Matrix)1430 inline lcMatrix44 lcMatrix44LDrawToLeoCAD(const lcMatrix44& Matrix)
1431 {
1432 lcMatrix44 m;
1433
1434 m.r[0] = lcVector4(Matrix[0][0], Matrix[0][2], -Matrix[0][1], 0.0f);
1435 m.r[1] = lcVector4(Matrix[2][0], Matrix[2][2], -Matrix[2][1], 0.0f);
1436 m.r[2] = lcVector4(-Matrix[1][0], -Matrix[1][2], Matrix[1][1], 0.0f);
1437 m.r[3] = lcVector4(Matrix[3][0], Matrix[3][2], -Matrix[3][1], 1.0f);
1438
1439 return m;
1440 }
1441
lcQuaternionRotationX(float Radians)1442 inline lcVector4 lcQuaternionRotationX(float Radians)
1443 {
1444 return lcVector4(sinf(Radians / 2.0f), 0, 0, cosf(Radians / 2.0f));
1445 }
1446
lcQuaternionRotationY(float Radians)1447 inline lcVector4 lcQuaternionRotationY(float Radians)
1448 {
1449 return lcVector4(0, sinf(Radians / 2.0f), 0, cosf(Radians / 2.0f));
1450 }
1451
lcQuaternionRotationZ(float Radians)1452 inline lcVector4 lcQuaternionRotationZ(float Radians)
1453 {
1454 return lcVector4(0, 0, sinf(Radians / 2.0f), cosf(Radians / 2.0f));
1455 }
1456
lcQuaternionFromAxisAngle(const lcVector4 & a)1457 inline lcVector4 lcQuaternionFromAxisAngle(const lcVector4& a)
1458 {
1459 const float s = sinf(a[3] / 2.0f);
1460 return lcVector4(a[0] * s, a[1] * s, a[2] * s, cosf(a[3] / 2.0f));
1461 }
1462
lcQuaternionToAxisAngle(const lcVector4 & a)1463 inline lcVector4 lcQuaternionToAxisAngle(const lcVector4& a)
1464 {
1465 const float Len = lcDot3(a, a);
1466
1467 if (Len > 0.00001f)
1468 {
1469 const float f = 1.0f / sqrtf(Len);
1470 return lcVector4(a[0] * f, a[1] * f, a[2] * f, acosf(a[3]) * 2.0f);
1471 }
1472 else
1473 {
1474 return lcVector4(0, 0, 1, 0);
1475 }
1476 }
1477
lcQuaternionMultiply(const lcVector4 & a,const lcVector4 & b)1478 inline lcVector4 lcQuaternionMultiply(const lcVector4& a, const lcVector4& b)
1479 {
1480 const float x = a[0] * b[3] + a[1] * b[2] - a[2] * b[1] + a[3] * b[0];
1481 const float y = -a[0] * b[2] + a[1] * b[3] + a[2] * b[0] + a[3] * b[1];
1482 const float z = a[0] * b[1] - a[1] * b[0] + a[2] * b[3] + a[3] * b[2];
1483 const float w = -a[0] * b[0] - a[1] * b[1] - a[2] * b[2] + a[3] * b[3];
1484
1485 return lcVector4(x, y, z, w);
1486 }
1487
lcQuaternionMul(const lcVector3 & a,const lcVector4 & b)1488 inline lcVector3 lcQuaternionMul(const lcVector3& a, const lcVector4& b)
1489 {
1490 // Faster to transform to a matrix and multiply.
1491 const float Tx = 2.0f*b[0];
1492 const float Ty = 2.0f*b[1];
1493 const float Tz = 2.0f*b[2];
1494 const float Twx = Tx*b[3];
1495 const float Twy = Ty*b[3];
1496 const float Twz = Tz*b[3];
1497 const float Txx = Tx*b[0];
1498 const float Txy = Ty*b[0];
1499 const float Txz = Tz*b[0];
1500 const float Tyy = Ty*b[1];
1501 const float Tyz = Tz*b[1];
1502 const float Tzz = Tz*b[2];
1503
1504 lcVector3 Rows[3];
1505 Rows[0] = lcVector3(1.0f-(Tyy+Tzz), Txy+Twz, Txz-Twy);
1506 Rows[1] = lcVector3(Txy-Twz, 1.0f-(Txx+Tzz), Tyz+Twx);
1507 Rows[2] = lcVector3(Txz+Twy, Tyz-Twx, 1.0f-(Txx+Tyy));
1508
1509 return lcVector3(Rows[0]*a[0] + Rows[1]*a[1] + Rows[2]*a[2]);
1510 }
1511
1512 // Convert world coordinates to screen coordinates.
lcProjectPoint(const lcVector3 & Point,const lcMatrix44 & ModelView,const lcMatrix44 & Projection,const int Viewport[4])1513 inline lcVector3 lcProjectPoint(const lcVector3& Point, const lcMatrix44& ModelView, const lcMatrix44& Projection, const int Viewport[4])
1514 {
1515 lcVector4 Tmp;
1516
1517 Tmp = lcMul4(lcVector4(Point[0], Point[1], Point[2], 1.0f), ModelView);
1518 Tmp = lcMul4(Tmp, Projection);
1519
1520 // Normalize.
1521 Tmp /= Tmp[3];
1522
1523 // Screen coordinates.
1524 return lcVector3(Viewport[0] + (1 + Tmp[0]) * Viewport[2] / 2, Viewport[1] + (1 + Tmp[1]) * Viewport[3] / 2, (1 + Tmp[2]) / 2);
1525 }
1526
lcUnprojectPoint(const lcVector3 & Point,const lcMatrix44 & ModelView,const lcMatrix44 & Projection,const int Viewport[4])1527 inline lcVector3 lcUnprojectPoint(const lcVector3& Point, const lcMatrix44& ModelView, const lcMatrix44& Projection, const int Viewport[4])
1528 {
1529 // Calculate the screen to model transform.
1530 const lcMatrix44 Transform = lcMatrix44Inverse(lcMul(ModelView, Projection));
1531
1532 lcVector4 Tmp;
1533
1534 // Convert the point to homogeneous coordinates.
1535 Tmp[0] = (Point[0] - Viewport[0]) * 2.0f / Viewport[2] - 1.0f;
1536 Tmp[1] = (Point[1] - Viewport[1]) * 2.0f / Viewport[3] - 1.0f;
1537 Tmp[2] = Point[2] * 2.0f - 1.0f;
1538 Tmp[3] = 1.0f;
1539
1540 Tmp = lcMul4(Tmp, Transform);
1541
1542 if (Tmp[3] != 0.0f)
1543 Tmp /= Tmp[3];
1544
1545 return lcVector3(Tmp[0], Tmp[1], Tmp[2]);
1546 }
1547
lcUnprojectPoints(lcVector3 * Points,int NumPoints,const lcMatrix44 & ModelView,const lcMatrix44 & Projection,const int Viewport[4])1548 inline void lcUnprojectPoints(lcVector3* Points, int NumPoints, const lcMatrix44& ModelView, const lcMatrix44& Projection, const int Viewport[4])
1549 {
1550 // Calculate the screen to model transform.
1551 const lcMatrix44 Transform = lcMatrix44Inverse(lcMul(ModelView, Projection));
1552
1553 for (int i = 0; i < NumPoints; i++)
1554 {
1555 lcVector4 Tmp;
1556
1557 // Convert the point to homogeneous coordinates.
1558 Tmp[0] = (Points[i][0] - Viewport[0]) * 2.0f / Viewport[2] - 1.0f;
1559 Tmp[1] = (Points[i][1] - Viewport[1]) * 2.0f / Viewport[3] - 1.0f;
1560 Tmp[2] = Points[i][2] * 2.0f - 1.0f;
1561 Tmp[3] = 1.0f;
1562
1563 Tmp = lcMul4(Tmp, Transform);
1564
1565 if (Tmp[3] != 0.0f)
1566 Tmp /= Tmp[3];
1567
1568 Points[i] = lcVector3(Tmp[0], Tmp[1], Tmp[2]);
1569 }
1570 }
1571
lcGetFrustumPlanes(const lcMatrix44 & WorldView,const lcMatrix44 & Projection,lcVector4 Planes[6])1572 inline void lcGetFrustumPlanes(const lcMatrix44& WorldView, const lcMatrix44& Projection, lcVector4 Planes[6])
1573 {
1574 lcMatrix44 WorldProj = lcMul(WorldView, Projection);
1575
1576 Planes[0][0] = (WorldProj[0][0] - WorldProj[0][3]) * -1;
1577 Planes[0][1] = (WorldProj[1][0] - WorldProj[1][3]) * -1;
1578 Planes[0][2] = (WorldProj[2][0] - WorldProj[2][3]) * -1;
1579 Planes[0][3] = (WorldProj[3][0] - WorldProj[3][3]) * -1;
1580 Planes[1][0] = WorldProj[0][0] + WorldProj[0][3];
1581 Planes[1][1] = WorldProj[1][0] + WorldProj[1][3];
1582 Planes[1][2] = WorldProj[2][0] + WorldProj[2][3];
1583 Planes[1][3] = WorldProj[3][0] + WorldProj[3][3];
1584 Planes[2][0] = (WorldProj[0][1] - WorldProj[0][3]) * -1;
1585 Planes[2][1] = (WorldProj[1][1] - WorldProj[1][3]) * -1;
1586 Planes[2][2] = (WorldProj[2][1] - WorldProj[2][3]) * -1;
1587 Planes[2][3] = (WorldProj[3][1] - WorldProj[3][3]) * -1;
1588 Planes[3][0] = WorldProj[0][1] + WorldProj[0][3];
1589 Planes[3][1] = WorldProj[1][1] + WorldProj[1][3];
1590 Planes[3][2] = WorldProj[2][1] + WorldProj[2][3];
1591 Planes[3][3] = WorldProj[3][1] + WorldProj[3][3];
1592 Planes[4][0] = (WorldProj[0][2] - WorldProj[0][3]) * -1;
1593 Planes[4][1] = (WorldProj[1][2] - WorldProj[1][3]) * -1;
1594 Planes[4][2] = (WorldProj[2][2] - WorldProj[2][3]) * -1;
1595 Planes[4][3] = (WorldProj[3][2] - WorldProj[3][3]) * -1;
1596 Planes[5][0] = WorldProj[0][2] + WorldProj[0][3];
1597 Planes[5][1] = WorldProj[1][2] + WorldProj[1][3];
1598 Planes[5][2] = WorldProj[2][2] + WorldProj[2][3];
1599 Planes[5][3] = WorldProj[3][2] + WorldProj[3][3];
1600
1601 for (int i = 0; i < 6; i++)
1602 {
1603 const lcVector3 Normal(Planes[i][0], Planes[i][1], Planes[i][2]);
1604 const float Length = Normal.Length();
1605 Planes[i] /= -Length;
1606 }
1607 }
1608
lcZoomExtents(const lcVector3 & Position,const lcMatrix44 & WorldView,const lcMatrix44 & Projection,const lcVector3 * Points,size_t NumPoints)1609 inline std::tuple<lcVector3, float> lcZoomExtents(const lcVector3& Position, const lcMatrix44& WorldView, const lcMatrix44& Projection, const lcVector3* Points, size_t NumPoints)
1610 {
1611 if (!NumPoints)
1612 return std::make_tuple(Position, 2500.0f);
1613
1614 lcVector4 Planes[6];
1615 lcGetFrustumPlanes(WorldView, Projection, Planes);
1616
1617 const lcVector3 Front(WorldView[0][2], WorldView[1][2], WorldView[2][2]);
1618
1619 float SmallestDistance = FLT_MAX;
1620
1621 for (int PlaneIdx = 0; PlaneIdx < 4; PlaneIdx++)
1622 {
1623 const lcVector3 Plane(Planes[PlaneIdx][0], Planes[PlaneIdx][1], Planes[PlaneIdx][2]);
1624 const float ep = lcDot(Position, Plane);
1625 const float fp = lcDot(Front, Plane);
1626
1627 for (size_t PointIdx = 0; PointIdx < NumPoints; PointIdx++)
1628 {
1629 const float u = (ep - lcDot(Points[PointIdx], Plane)) / fp;
1630
1631 if (u < SmallestDistance)
1632 SmallestDistance = u;
1633 }
1634 }
1635
1636 lcVector3 NewPosition = Position - (Front * SmallestDistance);
1637
1638 float FarDistance = 2500.0f;
1639
1640 for (size_t PointIdx = 0; PointIdx < NumPoints; PointIdx++)
1641 {
1642 const float Distance = lcDot(Points[PointIdx], Front);
1643
1644 if (Distance > FarDistance)
1645 FarDistance = Distance;
1646 }
1647
1648 return std::make_tuple(NewPosition, FarDistance + lcDot(NewPosition, Front));
1649 }
1650
lcClosestPointsBetweenLines(const lcVector3 & Line1a,const lcVector3 & Line1b,const lcVector3 & Line2a,const lcVector3 & Line2b,lcVector3 * Intersection1,lcVector3 * Intersection2)1651 inline void lcClosestPointsBetweenLines(const lcVector3& Line1a, const lcVector3& Line1b, const lcVector3& Line2a, const lcVector3& Line2b, lcVector3* Intersection1, lcVector3* Intersection2)
1652 {
1653 const lcVector3 u1 = Line1b - Line1a;
1654 const lcVector3 u2 = Line2b - Line2a;
1655 const lcVector3 p21 = Line2a - Line1a;
1656 const lcVector3 m = lcCross(u2, u1);
1657 const float m2 = lcDot(m, m);
1658
1659 if (m2 < 0.00001f)
1660 {
1661 if (Intersection1)
1662 *Intersection1 = Line1a;
1663 if (Intersection2)
1664 *Intersection2 = Line2a;
1665 return;
1666 }
1667
1668 const lcVector3 r = lcCross(p21, m / m2);
1669
1670 if (Intersection1)
1671 {
1672 const float t1 = lcDot(r, u2);
1673 *Intersection1 = Line1a + t1 * u1;
1674 }
1675
1676 if (Intersection2)
1677 {
1678 const float t2 = lcDot(r, u1);
1679 *Intersection2 = Line2a + t2 * u2;
1680 }
1681 }
1682
lcLineSegmentPlaneIntersection(lcVector3 * Intersection,const lcVector3 & Start,const lcVector3 & End,const lcVector4 & Plane)1683 inline bool lcLineSegmentPlaneIntersection(lcVector3* Intersection, const lcVector3& Start, const lcVector3& End, const lcVector4& Plane)
1684 {
1685 const lcVector3 Dir = End - Start;
1686 const lcVector3 PlaneNormal(Plane[0], Plane[1], Plane[2]);
1687
1688 const float t1 = lcDot(PlaneNormal, Start) + Plane[3];
1689 const float t2 = lcDot(PlaneNormal, Dir);
1690
1691 if (t2 == 0.0f)
1692 return false;
1693
1694 const float t = -t1 / t2;
1695
1696 *Intersection = Start + t * Dir;
1697
1698 if ((t < 0.0f) || (t > 1.0f))
1699 return false;
1700
1701 return true;
1702 }
1703
lcLineTriangleMinIntersection(const lcVector3 & p1,const lcVector3 & p2,const lcVector3 & p3,const lcVector3 & Start,const lcVector3 & End,float * MinDist,lcVector3 * Intersection)1704 inline bool lcLineTriangleMinIntersection(const lcVector3& p1, const lcVector3& p2, const lcVector3& p3, const lcVector3& Start, const lcVector3& End, float* MinDist, lcVector3* Intersection)
1705 {
1706 // Calculate the polygon plane.
1707 const lcVector3 PlaneNormal = lcCross(p1 - p2, p3 - p2);
1708 const float PlaneD = -lcDot(PlaneNormal, p1);
1709
1710 // Check if the line is parallel to the plane.
1711 const lcVector3 Dir = End - Start;
1712
1713 const float t1 = lcDot(PlaneNormal, Start) + PlaneD;
1714 const float t2 = lcDot(PlaneNormal, Dir);
1715
1716 if (t2 == 0)
1717 return false;
1718
1719 const float t = -(t1 / t2);
1720
1721 if (t < 0)
1722 return false;
1723
1724 // Intersection of the plane and line segment.
1725 *Intersection = Start - (t1 / t2) * Dir;
1726
1727 float Dist = lcLength(Start - *Intersection);
1728
1729 if (Dist > *MinDist)
1730 return false;
1731
1732 // Check if we're inside the triangle.
1733 lcVector3 pa1, pa2, pa3;
1734 pa1 = lcNormalize(p1 - *Intersection);
1735 pa2 = lcNormalize(p2 - *Intersection);
1736 pa3 = lcNormalize(p3 - *Intersection);
1737
1738 float a1, a2, a3;
1739 a1 = lcDot(pa1, pa2);
1740 a2 = lcDot(pa2, pa3);
1741 a3 = lcDot(pa3, pa1);
1742
1743 const float total = (acosf(a1) + acosf(a2) + acosf(a3)) * LC_RTOD;
1744
1745 if (fabs(total - 360) <= 0.001f)
1746 {
1747 *MinDist = Dist;
1748 return true;
1749 }
1750
1751 return false;
1752 }
1753
1754 // Sutherland-Hodgman method of clipping a polygon to a plane.
lcPolygonPlaneClip(lcVector3 * InPoints,int NumInPoints,lcVector3 * OutPoints,int * NumOutPoints,const lcVector4 & Plane)1755 inline void lcPolygonPlaneClip(lcVector3* InPoints, int NumInPoints, lcVector3* OutPoints, int* NumOutPoints, const lcVector4& Plane)
1756 {
1757 lcVector3 *s, *p, i;
1758
1759 *NumOutPoints = 0;
1760 s = &InPoints[NumInPoints-1];
1761
1762 for (int j = 0; j < NumInPoints; j++)
1763 {
1764 p = &InPoints[j];
1765
1766 if (lcDot3(*p, Plane) + Plane[3] <= 0)
1767 {
1768 if (lcDot3(*s, Plane) + Plane[3] <= 0)
1769 {
1770 // Both points inside.
1771 OutPoints[*NumOutPoints] = *p;
1772 *NumOutPoints = *NumOutPoints + 1;
1773 }
1774 else
1775 {
1776 // Outside, inside.
1777 lcLineSegmentPlaneIntersection(&i, *s, *p, Plane);
1778
1779 OutPoints[*NumOutPoints] = i;
1780 *NumOutPoints = *NumOutPoints + 1;
1781 OutPoints[*NumOutPoints] = *p;
1782 *NumOutPoints = *NumOutPoints + 1;
1783 }
1784 }
1785 else
1786 {
1787 if (lcDot3(*s, Plane) + Plane[3] <= 0)
1788 {
1789 // Inside, outside.
1790 lcLineSegmentPlaneIntersection(&i, *s, *p, Plane);
1791
1792 OutPoints[*NumOutPoints] = i;
1793 *NumOutPoints = *NumOutPoints + 1;
1794 }
1795 }
1796
1797 s = p;
1798 }
1799 }
1800
1801 // Return true if a polygon intersects a set of planes.
lcTriangleIntersectsPlanes(const float * p1,const float * p2,const float * p3,const lcVector4 Planes[6])1802 inline bool lcTriangleIntersectsPlanes(const float* p1, const float* p2, const float* p3, const lcVector4 Planes[6])
1803 {
1804 constexpr int NumPlanes = 6;
1805 const float* const Points[3] = { p1, p2, p3 };
1806 int Outcodes[3] = { 0, 0, 0 }, i;
1807 constexpr int NumPoints = 3;
1808
1809 // First do the Cohen-Sutherland out code test for trivial rejects/accepts.
1810 for (i = 0; i < NumPoints; i++)
1811 {
1812 const lcVector3 Pt(Points[i][0], Points[i][1], Points[i][2]);
1813
1814 for (int j = 0; j < NumPlanes; j++)
1815 {
1816 if (lcDot3(Pt, Planes[j]) + Planes[j][3] > 0)
1817 Outcodes[i] |= 1 << j;
1818 }
1819 }
1820
1821 // Polygon completely outside a plane.
1822 if ((Outcodes[0] & Outcodes[1] & Outcodes[2]) != 0)
1823 return false;
1824
1825 // If any vertex has an out code of all zeros then we intersect the volume.
1826 if (!Outcodes[0] || !Outcodes[1] || !Outcodes[2])
1827 return true;
1828
1829 // Buffers for clipping the polygon.
1830 lcVector3 ClipPoints[2][8];
1831 int NumClipPoints[2];
1832 int ClipBuffer = 0;
1833
1834 NumClipPoints[0] = NumPoints;
1835 ClipPoints[0][0] = lcVector3(p1[0], p1[1], p1[2]);
1836 ClipPoints[0][1] = lcVector3(p2[0], p2[1], p2[2]);
1837 ClipPoints[0][2] = lcVector3(p3[0], p3[1], p3[2]);
1838
1839 // Now clip the polygon against the planes.
1840 for (i = 0; i < NumPlanes; i++)
1841 {
1842 lcPolygonPlaneClip(ClipPoints[ClipBuffer], NumClipPoints[ClipBuffer], ClipPoints[ClipBuffer^1], &NumClipPoints[ClipBuffer^1], Planes[i]);
1843 ClipBuffer ^= 1;
1844
1845 if (!NumClipPoints[ClipBuffer])
1846 return false;
1847 }
1848
1849 return true;
1850 }
1851
1852 // Return true if a ray intersects a bounding box, and calculates the distance from the start of the ray (adapted from Graphics Gems).
lcBoundingBoxRayIntersectDistance(const lcVector3 & Min,const lcVector3 & Max,const lcVector3 & Start,const lcVector3 & End,float * Dist,lcVector3 * Intersection)1853 inline bool lcBoundingBoxRayIntersectDistance(const lcVector3& Min, const lcVector3& Max, const lcVector3& Start, const lcVector3& End, float* Dist, lcVector3* Intersection)
1854 {
1855 bool MiddleQuadrant[3];
1856 bool Inside = true;
1857 float CandidatePlane[3];
1858 float MaxT[3];
1859 int i;
1860
1861 // Find candidate planes.
1862 for (i = 0; i < 3; i++)
1863 {
1864 if (Start[i] < Min[i])
1865 {
1866 MiddleQuadrant[i] = false;
1867 CandidatePlane[i] = Min[i];
1868 Inside = false;
1869 }
1870 else if (Start[i] > Max[i])
1871 {
1872 MiddleQuadrant[i] = false;
1873 CandidatePlane[i] = Max[i];
1874 Inside = false;
1875 }
1876 else
1877 {
1878 MiddleQuadrant[i] = true;
1879 CandidatePlane[i] = 0.0f;
1880 }
1881 }
1882
1883 // Ray origin inside box.
1884 if (Inside)
1885 {
1886 *Dist = 0;
1887
1888 if (Intersection)
1889 *Intersection = Start;
1890
1891 return true;
1892 }
1893
1894 // Calculate T distances to candidate planes.
1895 lcVector3 Dir = End - Start;
1896
1897 for (i = 0; i < 3; i++)
1898 {
1899 if (!MiddleQuadrant[i] && Dir[i] != 0.0f)
1900 MaxT[i] = (CandidatePlane[i] - Start[i]) / Dir[i];
1901 else
1902 MaxT[i] = -1.0f;
1903 }
1904
1905 // Get largest of the MaxT's for final choice of intersection.
1906 int WhichPlane = 0;
1907 for (i = 1; i < 3; i++)
1908 if (MaxT[WhichPlane] < MaxT[i])
1909 WhichPlane = i;
1910
1911 // Check final candidate actually inside box.
1912 if (MaxT[WhichPlane] < 0.0f)
1913 return false;
1914
1915 lcVector3 Point;
1916
1917 for (i = 0; i < 3; i++)
1918 {
1919 if (WhichPlane != i)
1920 {
1921 Point[i] = Start[i] + MaxT[WhichPlane] * Dir[i];
1922 if (Point[i] < Min[i] || Point[i] > Max[i])
1923 return false;
1924 }
1925 else
1926 Point[i] = CandidatePlane[i];
1927 }
1928
1929 *Dist = lcLength(Point - Start);
1930
1931 if (Intersection)
1932 *Intersection = Point;
1933
1934 return true;
1935 }
1936
lcSphereRayMinIntersectDistance(const lcVector3 & Center,float Radius,const lcVector3 & Start,const lcVector3 & End,float * Dist)1937 inline bool lcSphereRayMinIntersectDistance(const lcVector3& Center, float Radius, const lcVector3& Start, const lcVector3& End, float* Dist)
1938 {
1939 const lcVector3 Dir = Center - Start;
1940 const float LengthSquaredDir = lcLengthSquared(Dir);
1941 const float RadiusSquared = Radius * Radius;
1942
1943 if (LengthSquaredDir < RadiusSquared)
1944 {
1945 // Ray origin inside sphere.
1946 *Dist = 0;
1947 return true;
1948 }
1949 else
1950 {
1951 const lcVector3 RayDir = End - Start;
1952 float t = lcDot(Dir, RayDir) / lcLengthSquared(RayDir);
1953
1954 // Ray points away from sphere.
1955 if (t < 0)
1956 return false;
1957
1958 const float c = (RadiusSquared - LengthSquaredDir) / lcLengthSquared(RayDir) + (t * t);
1959 if (c > 0)
1960 {
1961 *Dist = t - sqrtf(c);
1962 return true;
1963 }
1964
1965 return false;
1966 }
1967 }
1968
lcRayPointClosestPoint(const lcVector3 & Point,const lcVector3 & Start,const lcVector3 & End)1969 inline lcVector3 lcRayPointClosestPoint(const lcVector3& Point, const lcVector3& Start, const lcVector3& End)
1970 {
1971 const lcVector3 Dir = Point - Start;
1972 const lcVector3 RayDir = End - Start;
1973
1974 float t = lcDot(Dir, RayDir) / lcLengthSquared(RayDir);
1975 t = lcClamp(t, 0.0f, 1.0f);
1976
1977 return Start + t * RayDir;
1978 }
1979
lcRayPointDistance(const lcVector3 & Point,const lcVector3 & Start,const lcVector3 & End)1980 inline float lcRayPointDistance(const lcVector3& Point, const lcVector3& Start, const lcVector3& End)
1981 {
1982 const lcVector3 Closest = lcRayPointClosestPoint(Point, Start, End);
1983
1984 return lcLength(Closest - Point);
1985 }
1986
1987 // Returns true if the axis aligned box intersects the volume defined by planes.
lcBoundingBoxIntersectsVolume(const lcVector3 & Min,const lcVector3 & Max,const lcVector4 Planes[6])1988 inline bool lcBoundingBoxIntersectsVolume(const lcVector3& Min, const lcVector3& Max, const lcVector4 Planes[6])
1989 {
1990 constexpr int NumPlanes = 6;
1991 lcVector3 Points[8] =
1992 {
1993 Points[0] = lcVector3(Min[0], Min[1], Min[2]),
1994 Points[1] = lcVector3(Min[0], Max[1], Min[2]),
1995 Points[2] = lcVector3(Max[0], Max[1], Min[2]),
1996 Points[3] = lcVector3(Max[0], Min[1], Min[2]),
1997 Points[4] = lcVector3(Min[0], Min[1], Max[2]),
1998 Points[5] = lcVector3(Min[0], Max[1], Max[2]),
1999 Points[6] = lcVector3(Max[0], Max[1], Max[2]),
2000 Points[7] = lcVector3(Max[0], Min[1], Max[2])
2001 };
2002
2003 // Start by testing trivial reject/accept cases.
2004 int Outcodes[8];
2005 int i;
2006
2007 for (i = 0; i < 8; i++)
2008 {
2009 Outcodes[i] = 0;
2010
2011 for (int j = 0; j < NumPlanes; j++)
2012 {
2013 if (lcDot3(Points[i], Planes[j]) + Planes[j][3] > 0)
2014 Outcodes[i] |= 1 << j;
2015 }
2016 }
2017
2018 int OutcodesOR = 0, OutcodesAND = 0x3f;
2019
2020 for (i = 0; i < 8; i++)
2021 {
2022 OutcodesAND &= Outcodes[i];
2023 OutcodesOR |= Outcodes[i];
2024 }
2025
2026 // All corners outside the same plane.
2027 if (OutcodesAND != 0)
2028 return false;
2029
2030 // All corners inside the volume.
2031 if (OutcodesOR == 0)
2032 return true;
2033
2034 int Indices[36] =
2035 {
2036 0, 1, 2,
2037 0, 2, 3,
2038 7, 6, 5,
2039 7, 5, 4,
2040 0, 1, 5,
2041 0, 5, 4,
2042 2, 3, 7,
2043 2, 7, 6,
2044 0, 3, 7,
2045 0, 7, 4,
2046 1, 2, 6,
2047 1, 6, 5
2048 };
2049
2050 for (int Idx = 0; Idx < 36; Idx += 3)
2051 if (lcTriangleIntersectsPlanes(Points[Indices[Idx]*3], Points[Indices[Idx+1]*3], Points[Indices[Idx+2]*3], Planes))
2052 return true;
2053
2054 return false;
2055 }
2056
2057 struct lcBoundingBox
2058 {
2059 lcVector3 Min;
2060 lcVector3 Max;
2061 };
2062
lcGetBoxCorners(const lcVector3 & Min,const lcVector3 & Max,lcVector3 Points[8])2063 inline void lcGetBoxCorners(const lcVector3& Min, const lcVector3& Max, lcVector3 Points[8])
2064 {
2065 Points[0] = lcVector3(Max.x, Max.y, Min.z);
2066 Points[1] = lcVector3(Min.x, Max.y, Min.z);
2067 Points[2] = lcVector3(Max.x, Max.y, Max.z);
2068 Points[3] = lcVector3(Min.x, Min.y, Min.z);
2069 Points[4] = lcVector3(Min.x, Min.y, Max.z);
2070 Points[5] = lcVector3(Max.x, Min.y, Max.z);
2071 Points[6] = lcVector3(Max.x, Min.y, Min.z);
2072 Points[7] = lcVector3(Min.x, Max.y, Max.z);
2073 }
2074
lcGetBoxCorners(const lcBoundingBox & BoundingBox,lcVector3 Points[8])2075 inline void lcGetBoxCorners(const lcBoundingBox& BoundingBox, lcVector3 Points[8])
2076 {
2077 lcGetBoxCorners(BoundingBox.Min, BoundingBox.Max, Points);
2078 }
2079
2080 /*
2081 bool SphereIntersectsVolume(const Vector3& Center, float Radius, const Vector4* Planes, int NumPlanes)
2082 {
2083 for (int j = 0; j < NumPlanes; j++)
2084 if (Dot3(Center, Planes[j]) + Planes[j][3] > Radius)
2085 return false;
2086
2087 return true;
2088 }*/
2089
lcRGBToHSL(const lcVector3 & rgb)2090 inline lcVector3 lcRGBToHSL(const lcVector3& rgb)
2091 {
2092 int Mi;
2093 float M, m, C, h, S, L; // h is H/60
2094
2095 Mi = (rgb[0] >= rgb[1]) ? 0 : 1;
2096 Mi = (rgb[Mi] >= rgb[2]) ? Mi : 2;
2097 M = rgb[Mi];
2098
2099 m = (rgb[0] < rgb[1]) ? rgb[0] : rgb[1];
2100 m = (m < rgb[2]) ? m : rgb[2];
2101
2102 C = M - m;
2103 L = (M + m) / 2.0f;
2104
2105 if (C < LC_RGB_EPSILON) // C == 0.0
2106 h = 0.0f;
2107 else if (Mi == 0) // M == R
2108 h = 0.0f + (rgb[1] - rgb[2]) / C;
2109 else if (Mi == 1) // M == G
2110 h = 2.0f + (rgb[2] - rgb[0]) / C;
2111 else // M = B
2112 h = 4.0f + (rgb[0] - rgb[1]) / C;
2113
2114 h = (h < 0.0) ? h + 6.0f : h;
2115 h = (h >= 6.0) ? h - 6.0f : h;
2116
2117 S = ((L < (LC_RGB_EPSILON / 2.0f)) || (L > (1.0f -(LC_RGB_EPSILON / 2.0f))))
2118 ? 0.0f : (2.0f * (M - L)) / (1.0f - fabs((2.0f * L) - 1.0f)) ;
2119
2120 return lcVector3(h, S, L);
2121 }
2122
lcHSLToRGB(const lcVector3 & hSL)2123 inline lcVector3 lcHSLToRGB(const lcVector3& hSL)
2124 {
2125 lcVector3 rgb;
2126 float h, S, L, C, X, m;
2127
2128 h = hSL[0];
2129 S = hSL[1];
2130 L = hSL[2];
2131
2132 C = (1.0f - fabs(2.0f * L - 1.0f)) * S;
2133 X = C * (1.0f - fabs(fmodf(h, 2.0f) - 1.0f));
2134
2135 if (h < 1.0f)
2136 rgb = lcVector3(C, X, 0.0f);
2137 else if (h < 2.0f)
2138 rgb = lcVector3(X, C, 0.0f);
2139 else if (h < 3.0f)
2140 rgb = lcVector3(0.0f, C, X);
2141 else if (h < 4.0f)
2142 rgb = lcVector3(0.0f, X, C);
2143 else if (h < 5.0f)
2144 rgb = lcVector3(X, 0.0f, C);
2145 else
2146 rgb = lcVector3(C, 0.0f, X);
2147
2148 m = L - C / 2.0f;
2149 rgb += m;
2150
2151 return rgb;
2152 }
2153
lcAlgorithmicEdgeColor(const lcVector3 & Value,const float ValueLum,const float EdgeLum,const float Contrast,const float Saturation)2154 inline lcVector4 lcAlgorithmicEdgeColor(const lcVector3& Value, const float ValueLum, const float EdgeLum, const float Contrast, const float Saturation)
2155 {
2156 float y1, yt;
2157 lcVector3 hSL, rgb1, rgbf;
2158
2159 // Determine luma target
2160 if (EdgeLum < ValueLum)
2161 {
2162 // Light base color
2163 yt = ValueLum - Contrast * ValueLum;
2164 }
2165 else
2166 {
2167 // Dark base color
2168 yt = ValueLum + Contrast * (1.0f - ValueLum);
2169 }
2170
2171 // Get base color in hSL
2172 hSL = lcRGBToHSL(Value);
2173
2174 // Adjust saturation
2175 // sat = 4.0f * sat - 2.0f;
2176 // if (sat < 0.0f)
2177 // {
2178 // sat = -sat;
2179 // hSL[0] = (hSL[0] < 3.0f) ? hSL[0] + 3.0f : hSL[0] - 3.0f;
2180 // }
2181 // sat = (sat > 2.0f) ? 2.0f : sat;
2182 // if (sat > 1.0f)
2183 // {
2184 // // Supersaturate
2185 // sat -= 1.0f;
2186 // hSL[1] += sat * (1.0f - hSL[1]);
2187 // }
2188 // else
2189 // {
2190 // Desaturate
2191 hSL[1] *= Saturation;
2192 // }
2193
2194 // Adjusted color to RGB
2195 rgb1 = lcHSLToRGB(lcVector3(hSL[0], hSL[1], 0.5f));
2196
2197 // Fix adjusted color luma to target value
2198 y1 = lcLuminescence(rgb1);
2199
2200 if (yt < y1)
2201 {
2202 // Make darker via scaling
2203 rgbf = (yt/y1) * rgb1;
2204 }
2205 else
2206 {
2207 // Make lighter via scaling anti-color
2208 rgbf = lcVector3(1.0f, 1.0f, 1.0f) - rgb1;
2209 rgbf *= (1.0f - yt) / (1.0f - y1);
2210 rgbf = lcVector3(1.0f, 1.0f, 1.0f) - rgbf;
2211 }
2212
2213 return lcVector4(lcLinearToSRGB(rgbf), 1.0f);
2214 }
2215