LCOV - code coverage report
Current view: top level - shared - matrix.cpp (source / functions) Coverage Total Hit
Test: Libprimis Test Coverage Lines: 93.8 % 663 622
Test Date: 2025-02-18 06:21:28 Functions: 95.5 % 154 147

            Line data    Source code
       1              : #include "../libprimis-headers/cube.h"
       2              : #include "geomexts.h"
       3              : 
       4              : /* This file defines matrix3, matrix4, and matrix4x3's member functions.
       5              :  * The definitions of the classes themselves are in geom.h (one of the shared
       6              :  * header files)
       7              :  *
       8              :  */
       9              : 
      10              : //needed for det3
      11              : /* det2x2()
      12              :  *
      13              :  * returns the determinant of a 2x2 matrix, provided a set of four doubles
      14              :  * (rather than as a matrix2 object)
      15              :  */
      16          120 : static double det2x2(double a, double b, double c, double d)
      17              : {
      18          120 :     return a*d - b*c;
      19              : }
      20              : 
      21              : //needed to invert matrix below
      22              : /* det3x3()
      23              :  *
      24              :  * returns the determinant of a 3x3 matrix, provided a set of nine doubles
      25              :  * (rather than as a matrix3 object)
      26              :  */
      27           40 : static double det3x3(double a1, double a2, double a3,
      28              :                             double b1, double b2, double b3,
      29              :                             double c1, double c2, double c3)
      30              : {
      31           40 :     return a1 * det2x2(b2, b3, c2, c3)
      32           40 :          - b1 * det2x2(a2, a3, c2, c3)
      33           40 :          + c1 * det2x2(a2, a3, b2, b3);
      34              : }
      35              : 
      36              : // =============================================================================
      37              : //  matrix3 (3x3) object
      38              : // =============================================================================
      39              : 
      40          509 : matrix3::matrix3() : a(0,0,0), b(0,0,0), c(0,0,0)
      41              : {
      42          509 : }
      43              : 
      44           22 : matrix3::matrix3(const vec &a, const vec &b, const vec &c) : a(a), b(b), c(c)
      45              : {
      46           22 : }
      47              : 
      48            4 : matrix3::matrix3(const quat &q)
      49              : {
      50            4 :     float x   = q.x,   y  = q.y,    z = q.z, w = q.w,
      51            4 :           tx  = 2*x,   ty = 2*y,   tz = 2*z,
      52            4 :           txx = tx*x, tyy = ty*y, tzz = tz*z,
      53            4 :           txy = tx*y, txz = tx*z, tyz = ty*z,
      54            4 :           twx = w*tx, twy = w*ty, twz = w*tz;
      55            4 :     a = vec(1 - (tyy + tzz), txy + twz, txz - twy);
      56            4 :     b = vec(txy - twz, 1 - (txx + tzz), tyz + twx);
      57            4 :     c = vec(txz + twy, tyz - twx, 1 - (txx + tyy));
      58            4 : }
      59              : 
      60            9 : matrix3::matrix3(float angle, const vec &axis)
      61              : {
      62            9 :     rotate(angle, axis);
      63            9 : }
      64              : 
      65            4 : void matrix3::mul(const matrix3 &m, const matrix3 &n)
      66              : {
      67            4 :     a = vec(m.a).mul(n.a.x).madd(m.b, n.a.y).madd(m.c, n.a.z);
      68            4 :     b = vec(m.a).mul(n.b.x).madd(m.b, n.b.y).madd(m.c, n.b.z);
      69            4 :     c = vec(m.a).mul(n.c.x).madd(m.b, n.c.y).madd(m.c, n.c.z);
      70            4 : }
      71              : 
      72            2 : void matrix3::mul(const matrix3 &n)
      73              : {
      74            2 :     mul(matrix3(*this), n);
      75            2 : }
      76              : 
      77            2 : void matrix3::multranspose(const matrix3 &m, const matrix3 &n)
      78              : {
      79            2 :     a = vec(m.a).mul(n.a.x).madd(m.b, n.b.x).madd(m.c, n.c.x);
      80            2 :     b = vec(m.a).mul(n.a.y).madd(m.b, m.b.y).madd(m.c, n.c.y);
      81            2 :     c = vec(m.a).mul(n.a.z).madd(m.b, n.b.z).madd(m.c, n.c.z);
      82            2 : }
      83            2 : void matrix3::multranspose(const matrix3 &n)
      84              : {
      85            2 :     multranspose(matrix3(*this), n);
      86            2 : }
      87              : 
      88            2 : void matrix3::transposemul(const matrix3 &m, const matrix3 &n)
      89              : {
      90            2 :     a = vec(m.a.dot(n.a), m.b.dot(n.a), m.c.dot(n.a));
      91            2 :     b = vec(m.a.dot(n.b), m.b.dot(n.b), m.c.dot(n.b));
      92            2 :     c = vec(m.a.dot(n.c), m.b.dot(n.c), m.c.dot(n.c));
      93            2 : }
      94              : 
      95            2 : void matrix3::transposemul(const matrix3 &n)
      96              : {
      97            2 :     transposemul(matrix3(*this), n);
      98            2 : }
      99              : 
     100            1 : void matrix3::transpose()
     101              : {
     102            1 :     std::swap(a.y, b.x); std::swap(a.z, c.x);
     103            1 :     std::swap(b.z, c.y);
     104            1 : }
     105              : 
     106            4 : void matrix3::transpose(const matrix3 &m)
     107              : {
     108            4 :     a = vec(m.a.x, m.b.x, m.c.x);
     109            4 :     b = vec(m.a.y, m.b.y, m.c.y);
     110            4 :     c = vec(m.a.z, m.b.z, m.c.z);
     111            4 : }
     112              : 
     113            4 : void matrix3::invert(const matrix3 &o)
     114              : {
     115            4 :     vec unscale(1/o.a.squaredlen(), 1/o.b.squaredlen(), 1/o.c.squaredlen());
     116            4 :     transpose(o);
     117            4 :     a.mul(unscale);
     118            4 :     b.mul(unscale);
     119            4 :     c.mul(unscale);
     120            4 : }
     121              : 
     122            2 : void matrix3::invert()
     123              : {
     124            2 :     invert(matrix3(*this));
     125            2 : }
     126              : 
     127            2 : void matrix3::normalize()
     128              : {
     129            2 :     a.normalize();
     130            2 :     b.normalize();
     131            2 :     c.normalize();
     132            2 : }
     133              : 
     134            1 : void matrix3::scale(float k)
     135              : {
     136            1 :     a.mul(k);
     137            1 :     b.mul(k);
     138            1 :     c.mul(k);
     139            1 : }
     140              : 
     141            9 : void matrix3::rotate(float angle, const vec &axis)
     142              : {
     143            9 :     rotate(cosf(angle), std::sin(angle), axis);
     144            9 : }
     145              : 
     146           24 : void matrix3::rotate(float ck, float sk, const vec &axis)
     147              : {
     148           24 :     a = vec(axis.x*axis.x*(1-ck)+ck, axis.x*axis.y*(1-ck)+axis.z*sk, axis.x*axis.z*(1-ck)-axis.y*sk);
     149           24 :     b = vec(axis.x*axis.y*(1-ck)-axis.z*sk, axis.y*axis.y*(1-ck)+ck, axis.y*axis.z*(1-ck)+axis.x*sk);
     150           24 :     c = vec(axis.x*axis.z*(1-ck)+axis.y*sk, axis.y*axis.z*(1-ck)-axis.x*sk, axis.z*axis.z*(1-ck)+ck);
     151           24 : }
     152              : 
     153            2 : void matrix3::setyaw(float ck, float sk)
     154              : {
     155            2 :     a = vec(ck, sk, 0);
     156            2 :     b = vec(-sk, ck, 0);
     157            2 :     c = vec(0, 0, 1);
     158            2 : }
     159              : 
     160            2 : void matrix3::setyaw(float angle)
     161              : {
     162            2 :     setyaw(cosf(angle), std::sin(angle));
     163            2 : }
     164              : 
     165           10 : float matrix3::trace() const
     166              : {
     167           10 :     return a.x + b.y + c.z;
     168              : }
     169              : 
     170            7 : bool matrix3::calcangleaxis(float tr, float &angle, vec &axis, float threshold) const
     171              : {
     172            7 :     if(tr <= -1)
     173              :     {
     174            2 :         if(a.x >= b.y && a.x >= c.z)
     175              :         {
     176            2 :             float r = 1 + a.x - b.y - c.z;
     177            2 :             if(r <= threshold)
     178              :             {
     179            0 :                 return false;
     180              :             }
     181            2 :             r = sqrtf(r);
     182            2 :             axis.x = 0.5f*r;
     183            2 :             axis.y = b.x/r;
     184            2 :             axis.z = c.x/r;
     185            2 :         }
     186            0 :         else if(b.y >= c.z)
     187              :         {
     188            0 :             float r = 1 + b.y - a.x - c.z;
     189            0 :             if(r <= threshold)
     190              :             {
     191            0 :                 return false;
     192              :             }
     193            0 :             r = sqrtf(r);
     194            0 :             axis.y = 0.5f*r;
     195            0 :             axis.x = b.x/r;
     196            0 :             axis.z = c.y/r;
     197              :         }
     198              :         else
     199              :         {
     200            0 :             float r = 1 + b.y - a.x - c.z;
     201            0 :             if(r <= threshold)
     202              :             {
     203            0 :                 return false;
     204              :             }
     205            0 :             r = sqrtf(r);
     206            0 :             axis.z = 0.5f*r;
     207            0 :             axis.x = c.x/r;
     208            0 :             axis.y = c.y/r;
     209              :         }
     210            2 :         angle = M_PI;
     211              :     }
     212            5 :     else if(tr >= 3)
     213              :     {
     214            2 :         axis = vec(0, 0, 1);
     215            2 :         angle = 0;
     216              :     }
     217              :     else
     218              :     {
     219            3 :         axis = vec(b.z - c.y, c.x - a.z, a.y - b.x);
     220            3 :         float r = axis.squaredlen();
     221            3 :         if(r <= threshold)
     222              :         {
     223            0 :             return false;
     224              :         }
     225            3 :         axis.mul(1/sqrtf(r));
     226            3 :         angle = acosf(0.5f*(tr - 1));
     227              :     }
     228            7 :     return true;
     229              : }
     230              : 
     231            1 : bool matrix3::calcangleaxis(float &angle, vec &axis, float threshold) const
     232              : {
     233            1 :     return calcangleaxis(trace(), angle, axis, threshold);
     234              : }
     235              : 
     236            5 : vec matrix3::transform(const vec &o) const
     237              : {
     238            5 :     return vec(a).mul(o.x).madd(b, o.y).madd(c, o.z);
     239              : }
     240              : 
     241            5 : vec matrix3::transposedtransform(const vec &o) const
     242              : {
     243            5 :     return vec(a.dot(o), b.dot(o), c.dot(o));
     244              : }
     245              : 
     246            4 : vec matrix3::abstransform(const vec &o) const
     247              : {
     248            4 :     return vec(a).mul(o.x).abs().add(vec(b).mul(o.y).abs()).add(vec(c).mul(o.z).abs());
     249              : }
     250              : 
     251            4 : vec matrix3::abstransposedtransform(const vec &o) const
     252              : {
     253            4 :     return vec(a.absdot(o), b.absdot(o), c.absdot(o));
     254              : }
     255              : 
     256           56 : void matrix3::identity()
     257              : {
     258           56 :     a = vec(1, 0, 0);
     259           56 :     b = vec(0, 1, 0);
     260           56 :     c = vec(0, 0, 1);
     261           56 : }
     262              : 
     263            2 : void matrix3::rotate_around_x(float ck, float sk)
     264              : {
     265            2 :     vec rb = vec(b).mul(ck).madd(c, sk),
     266            2 :         rc = vec(c).mul(ck).msub(b, sk);
     267            2 :     b = rb;
     268            2 :     c = rc;
     269            2 : }
     270              : 
     271            1 : void matrix3::rotate_around_x(float angle)
     272              : {
     273            1 :     rotate_around_x(cosf(angle), std::sin(angle));
     274            1 : }
     275              : 
     276            1 : void matrix3::rotate_around_x(const vec2 &sc)
     277              : {
     278            1 :     rotate_around_x(sc.x, sc.y);
     279            1 : }
     280              : 
     281            2 : void matrix3::rotate_around_y(float ck, float sk)
     282              : {
     283            2 :     vec rc = vec(c).mul(ck).madd(a, sk),
     284            2 :         ra = vec(a).mul(ck).msub(c, sk);
     285            2 :     c = rc;
     286            2 :     a = ra;
     287            2 : }
     288              : 
     289            1 : void matrix3::rotate_around_y(float angle)
     290              : {
     291            1 :     rotate_around_y(cosf(angle), std::sin(angle));
     292            1 : }
     293              : 
     294            1 : void matrix3::rotate_around_y(const vec2 &sc)
     295              : {
     296            1 :     rotate_around_y(sc.x, sc.y);
     297            1 : }
     298              : 
     299            2 : void matrix3::rotate_around_z(float ck, float sk)
     300              : {
     301            2 :     vec ra = vec(a).mul(ck).madd(b, sk),
     302            2 :         rb = vec(b).mul(ck).msub(a, sk);
     303            2 :     a = ra;
     304            2 :     b = rb;
     305            2 : }
     306              : 
     307            1 : void matrix3::rotate_around_z(float angle)
     308              : {
     309            1 :     rotate_around_z(cosf(angle), std::sin(angle));
     310            1 : }
     311              : 
     312            1 : void matrix3::rotate_around_z(const vec2 &sc)
     313              : {
     314            1 :     rotate_around_z(sc.x, sc.y);
     315            1 : }
     316              : 
     317            3 : vec matrix3::transform(const vec2 &o) const
     318              : {
     319            3 :     return vec(a).mul(o.x).madd(b, o.y);
     320              : }
     321              : 
     322            3 : vec matrix3::transposedtransform(const vec2 &o) const
     323              : {
     324            3 :     return vec(a.dot2(o), b.dot2(o), c.dot2(o));
     325              : }
     326              : 
     327            1 : vec matrix3::rowx() const
     328              : {
     329            1 :     return vec(a.x, b.x, c.x);
     330              : }
     331              : 
     332            1 : vec matrix3::rowy() const
     333              : {
     334            1 :     return vec(a.y, b.y, c.y);
     335              : }
     336              : 
     337            1 : vec matrix3::rowz() const
     338              : {
     339            1 :     return vec(a.z, b.z, c.z);
     340              : }
     341              : 
     342              : // =============================================================================
     343              : //  matrix4 (4x4) object
     344              : // =============================================================================
     345              : 
     346              : /* invert()
     347              :  *
     348              :  * sets the matrix values to the inverse of the provided matrix A*A^-1 = I
     349              :  * returns false if singular (or nearly singular to within tolerance of mindet)
     350              :  * or true if matrix was inverted successfully
     351              :  *
     352              :  * &m: a matrix4 object to be inverted and assigned to the object
     353              :  * mindet: the minimum value at which matrices are considered
     354              :  */
     355            4 : bool matrix4::invert(const matrix4 &m, double mindet)
     356              : {
     357            4 :     double a1 = m.a.x, a2 = m.a.y, a3 = m.a.z, a4 = m.a.w,
     358            4 :            b1 = m.b.x, b2 = m.b.y, b3 = m.b.z, b4 = m.b.w,
     359            4 :            c1 = m.c.x, c2 = m.c.y, c3 = m.c.z, c4 = m.c.w,
     360            4 :            d1 = m.d.x, d2 = m.d.y, d3 = m.d.z, d4 = m.d.w,
     361            4 :            det1 =  det3x3(b2, b3, b4, c2, c3, c4, d2, d3, d4),
     362            4 :            det2 = -det3x3(a2, a3, a4, c2, c3, c4, d2, d3, d4),
     363            4 :            det3 =  det3x3(a2, a3, a4, b2, b3, b4, d2, d3, d4),
     364            4 :            det4 = -det3x3(a2, a3, a4, b2, b3, b4, c2, c3, c4),
     365            4 :            det = a1*det1 + b1*det2 + c1*det3 + d1*det4;
     366              : 
     367            4 :     if(std::fabs(det) < mindet)
     368              :     {
     369            2 :         return false;
     370              :     }
     371              : 
     372            2 :     double invdet = 1/det;
     373              : 
     374            2 :     a.x = det1 * invdet;
     375            2 :     a.y = det2 * invdet;
     376            2 :     a.z = det3 * invdet;
     377            2 :     a.w = det4 * invdet;
     378              : 
     379            2 :     b.x = -det3x3(b1, b3, b4, c1, c3, c4, d1, d3, d4) * invdet;
     380            2 :     b.y =  det3x3(a1, a3, a4, c1, c3, c4, d1, d3, d4) * invdet;
     381            2 :     b.z = -det3x3(a1, a3, a4, b1, b3, b4, d1, d3, d4) * invdet;
     382            2 :     b.w =  det3x3(a1, a3, a4, b1, b3, b4, c1, c3, c4) * invdet;
     383              : 
     384            2 :     c.x =  det3x3(b1, b2, b4, c1, c2, c4, d1, d2, d4) * invdet;
     385            2 :     c.y = -det3x3(a1, a2, a4, c1, c2, c4, d1, d2, d4) * invdet;
     386            2 :     c.z =  det3x3(a1, a2, a4, b1, b2, b4, d1, d2, d4) * invdet;
     387            2 :     c.w = -det3x3(a1, a2, a4, b1, b2, b4, c1, c2, c4) * invdet;
     388              : 
     389            2 :     d.x = -det3x3(b1, b2, b3, c1, c2, c3, d1, d2, d3) * invdet;
     390            2 :     d.y =  det3x3(a1, a2, a3, c1, c2, c3, d1, d2, d3) * invdet;
     391            2 :     d.z = -det3x3(a1, a2, a3, b1, b2, b3, d1, d2, d3) * invdet;
     392            2 :     d.w =  det3x3(a1, a2, a3, b1, b2, b3, c1, c2, c3) * invdet;
     393              : 
     394            2 :     return true;
     395              : }
     396              : 
     397            2 : matrix4 matrix4::inverse(double mindet) const
     398              : {
     399            2 :     matrix4 ret;
     400            2 :     if(ret.invert(*this, mindet))
     401              :     {
     402            1 :         return ret;
     403              :     }
     404              :     else
     405              :     {
     406            1 :         return { vec4<float>(0,0,0,0), vec4<float>(0,0,0,0), vec4<float>(0,0,0,0), vec4<float>(0,0,0,0) };
     407              :     }
     408              : }
     409              : 
     410          137 : matrix4::matrix4() : a(0,0,0,0), b(0,0,0,0), c(0,0,0,0), d(0,0,0,0)
     411              : {
     412          137 : }
     413              : 
     414            1 : matrix4::matrix4(const float *m) : a(m), b(m+4), c(m+8), d(m+12)
     415              : {
     416            1 : }
     417              : 
     418            8 : matrix4::matrix4(const vec &a, const vec &b, const vec &c)
     419            8 :     : a(a.x, b.x, c.x, 0), b(a.y, b.y, c.y, 0), c(a.z, b.z, c.z, 0), d(0, 0, 0, 1)
     420              : {
     421            8 : }
     422              : 
     423           31 : matrix4::matrix4(const vec4<float> &a, const vec4<float> &b, const vec4<float> &c, const vec4<float> &d)
     424           31 :     : a(a), b(b), c(c), d(d)
     425              : {
     426           31 : }
     427              : 
     428            1 : matrix4::matrix4(const matrix4x3 &m)
     429            1 :     : a(m.a, 0), b(m.b, 0), c(m.c, 0), d(m.d, 1)
     430              : {
     431            1 : }
     432              : 
     433            1 : matrix4::matrix4(const matrix3 &rot, const vec &trans)
     434            1 :     : a(rot.a, 0), b(rot.b, 0), c(rot.c, 0), d(trans, 1)
     435              : {
     436            1 : }
     437              : 
     438            0 : void matrix4::transposedtransform(const plane &in, plane &out) const
     439              : {
     440            0 :     out.x = in.dist(a);
     441            0 :     out.y = in.dist(b);
     442            0 :     out.z = in.dist(c);
     443            0 :     out.offset = in.dist(d);
     444            0 : }
     445              : 
     446            9 : void matrix4::mul(const matrix4 &x, const matrix3 &y)
     447              : {
     448            9 :     a = vec4<float>(x.a).mul(y.a.x).madd(x.b, y.a.y).madd(x.c, y.a.z);
     449            9 :     b = vec4<float>(x.a).mul(y.b.x).madd(x.b, y.b.y).madd(x.c, y.b.z);
     450            9 :     c = vec4<float>(x.a).mul(y.c.x).madd(x.b, y.c.y).madd(x.c, y.c.z);
     451            9 :     d = x.d;
     452            9 : }
     453              : 
     454            9 : void matrix4::mul(const matrix3 &y)
     455              : {
     456            9 :     mul(matrix4(*this), y);
     457            9 : }
     458              : 
     459            1 : void matrix4::mul(const matrix4 &x, const matrix4 &y)
     460              : {
     461            1 :     mult<vec4<float>>(x, y);
     462            1 : }
     463              : 
     464            1 : void matrix4::mul(const matrix4 &y)
     465              : {
     466            1 :     mult<vec4<float>>(matrix4(*this), y);
     467            1 : }
     468              : 
     469            1 : void matrix4::muld(const matrix4 &x, const matrix4 &y)
     470              : {
     471            1 :     mult<vec4<double>>(x, y);
     472            1 : }
     473              : 
     474            1 : void matrix4::muld(const matrix4 &y)
     475              : {
     476            1 :     mult<vec4<double>>(matrix4(*this), y);
     477            1 : }
     478              : 
     479            2 : void matrix4::rotate_around_x(float ck, float sk)
     480              : {
     481            2 :     vec4<float> rb = vec4<float>(b).mul(ck).madd(c, sk),
     482            2 :                 rc = vec4<float>(c).mul(ck).msub(b, sk);
     483            2 :     b = rb;
     484            2 :     c = rc;
     485            2 : }
     486              : 
     487            1 : void matrix4::rotate_around_x(float angle)
     488              : {
     489            1 :     rotate_around_x(cosf(angle), std::sin(angle));
     490            1 : }
     491              : 
     492            1 : void matrix4::rotate_around_x(const vec2 &sc)
     493              : {
     494            1 :     rotate_around_x(sc.x, sc.y);
     495            1 : }
     496              : 
     497            2 : void matrix4::rotate_around_y(float ck, float sk)
     498              : {
     499            2 :     vec4<float> rc = vec4<float>(c).mul(ck).madd(a, sk),
     500            2 :                 ra = vec4<float>(a).mul(ck).msub(c, sk);
     501            2 :     c = rc;
     502            2 :     a = ra;
     503            2 : }
     504              : 
     505            1 : void matrix4::rotate_around_y(float angle)
     506              : {
     507            1 :     rotate_around_y(cosf(angle), std::sin(angle));
     508            1 : }
     509              : 
     510            1 : void matrix4::rotate_around_y(const vec2 &sc)
     511              : {
     512            1 :     rotate_around_y(sc.x, sc.y);
     513            1 : }
     514              : 
     515            2 : void matrix4::rotate_around_z(float ck, float sk)
     516              : {
     517            2 :     vec4<float> ra = vec4<float>(a).mul(ck).madd(b, sk),
     518            2 :                 rb = vec4<float>(b).mul(ck).msub(a, sk);
     519            2 :     a = ra;
     520            2 :     b = rb;
     521            2 : }
     522              : 
     523            1 : void matrix4::rotate_around_z(float angle)
     524              : {
     525            1 :     rotate_around_z(cosf(angle), std::sin(angle));
     526            1 : }
     527              : 
     528            1 : void matrix4::rotate_around_z(const vec2 &sc)
     529              : {
     530            1 :     rotate_around_z(sc.x, sc.y);
     531            1 : }
     532              : 
     533            9 : void matrix4::rotate(float ck, float sk, const vec &axis)
     534              : {
     535            9 :     matrix3 m;
     536            9 :     m.rotate(ck, sk, axis);
     537            9 :     mul(m);
     538            9 : }
     539              : 
     540            3 : void matrix4::rotate(float angle, const vec &dir)
     541              : {
     542            3 :     rotate(cosf(angle), std::sin(angle), dir);
     543            3 : }
     544              : 
     545            3 : void matrix4::rotate(const vec2 &sc, const vec &dir)
     546              : {
     547            3 :     rotate(sc.x, sc.y, dir);
     548            3 : }
     549              : 
     550           33 : void matrix4::identity()
     551              : {
     552           33 :     a = vec4<float>(1, 0, 0, 0);
     553           33 :     b = vec4<float>(0, 1, 0, 0);
     554           33 :     c = vec4<float>(0, 0, 1, 0);
     555           33 :     d = vec4<float>(0, 0, 0, 1);
     556           33 : }
     557              : 
     558            1 : void matrix4::settranslation(const vec &v)
     559              : {
     560            1 :     d.setxyz(v);
     561            1 : }
     562              : 
     563            1 : void matrix4::settranslation(float x, float y, float z)
     564              : {
     565            1 :     d.x = x;
     566            1 :     d.y = y;
     567            1 :     d.z = z;
     568            1 : }
     569              : 
     570            0 : void matrix4::translate(const vec &p)
     571              : {
     572            0 :     d.madd(a, p.x).madd(b, p.y).madd(c, p.z);
     573            0 : }
     574              : 
     575            0 : void matrix4::translate(float x, float y, float z)
     576              : {
     577            0 :     translate(vec(x, y, z));
     578            0 : }
     579              : 
     580            0 : void matrix4::translate(const vec &p, float scale)
     581              : {
     582            0 :     translate(vec(p).mul(scale));
     583            0 : }
     584              : 
     585            3 : void matrix4::setscale(float x, float y, float z)
     586              : {
     587            3 :     a.x = x;
     588            3 :     b.y = y;
     589            3 :     c.z = z;
     590            3 : }
     591              : 
     592            1 : void matrix4::setscale(const vec &v)
     593              : {
     594            1 :     setscale(v.x, v.y, v.z);
     595            1 : }
     596              : 
     597            1 : void matrix4::setscale(float n)
     598              : {
     599            1 :     setscale(n, n, n);
     600            1 : }
     601              : 
     602            3 : void matrix4::scale(float x, float y, float z)
     603              : {
     604            3 :     a.mul(x);
     605            3 :     b.mul(y);
     606            3 :     c.mul(z);
     607            3 : }
     608              : 
     609            1 : void matrix4::scale(const vec &v)
     610              : {
     611            1 :     scale(v.x, v.y, v.z);
     612            1 : }
     613              : 
     614            1 : void matrix4::scale(float n)
     615              : {
     616            1 :     scale(n, n, n);
     617            1 : }
     618              : 
     619            2 : void matrix4::scalez(float k)
     620              : {
     621            2 :     a.z *= k;
     622            2 :     b.z *= k;
     623            2 :     c.z *= k;
     624            2 :     d.z *= k;
     625            2 : }
     626              : 
     627            2 : void matrix4::jitter(float x, float y)
     628              : {
     629            2 :     a.x += x * a.w;
     630            2 :     a.y += y * a.w;
     631            2 :     b.x += x * b.w;
     632            2 :     b.y += y * b.w;
     633            2 :     c.x += x * c.w;
     634            2 :     c.y += y * c.w;
     635            2 :     d.x += x * d.w;
     636            2 :     d.y += y * d.w;
     637            2 : }
     638              : 
     639            1 : void matrix4::transpose()
     640              : {
     641              :     //swap upper triangular elements of row 'a'
     642            1 :     std::swap(a.y, b.x);
     643            1 :     std::swap(a.z, c.x);
     644            1 :     std::swap(a.w, d.x);
     645              :     //swap upper triangular elements of row 'b'
     646            1 :     std::swap(b.z, c.y);
     647            1 :     std::swap(b.w, d.y);
     648              :     //swap upper triangular element of row 'c'
     649            1 :     std::swap(c.w, d.z);
     650              :     //no upper triangular elements on row 'd'
     651            1 : }
     652              : 
     653            1 : void matrix4::transpose(const matrix4 &m)
     654              : {
     655            1 :     a = vec4<float>(m.a.x, m.b.x, m.c.x, m.d.x);
     656            1 :     b = vec4<float>(m.a.y, m.b.y, m.c.y, m.d.y);
     657            1 :     c = vec4<float>(m.a.z, m.b.z, m.c.z, m.d.z);
     658            1 :     d = vec4<float>(m.a.w, m.b.w, m.c.w, m.d.w);
     659            1 : }
     660              : 
     661            2 : void matrix4::frustum(float left, float right, float bottom, float top, float znear, float zfar)
     662              : {
     663            2 :     float width = right - left,
     664            2 :           height = top - bottom,
     665            2 :           zrange = znear - zfar;
     666            2 :     a = vec4<float>(2*znear/width, 0, 0, 0); //depth to width ratio
     667            2 :     b = vec4<float>(0, 2*znear/height, 0, 0); //depth to height ratio
     668            2 :     c = vec4<float>((right + left)/width, (top + bottom)/height, (zfar + znear)/zrange, -1); //offset from centered
     669            2 :     d = vec4<float>(0, 0, 2*znear*zfar/zrange, 0); //depth scale
     670            2 : }
     671              : 
     672            1 : void matrix4::perspective(float fovy, float aspect, float znear, float zfar)
     673              : {
     674            1 :     float ydist = znear * std::tan(fovy/(2*RAD)),
     675            1 :           xdist = ydist * aspect;
     676            1 :     frustum(-xdist, xdist, -ydist, ydist, znear, zfar);
     677            1 : }
     678              : 
     679            3 : void matrix4::ortho(float left, float right, float bottom, float top, float znear, float zfar)
     680              : {
     681            3 :     float width = right - left,
     682            3 :           height = top - bottom,
     683            3 :           zrange = znear - zfar;
     684            3 :     a = vec4<float>(2/width, 0, 0, 0);
     685            3 :     b = vec4<float>(0, 2/height, 0, 0);
     686            3 :     c = vec4<float>(0, 0, 2/zrange, 0);
     687            3 :     d = vec4<float>(-(right+left)/width, -(top+bottom)/height, (zfar+znear)/zrange, 1);
     688            3 : }
     689              : 
     690            1 : void matrix4::transform(const vec &in, vec &out) const
     691              : {
     692            1 :     out = vec(a).mul(in.x).add(vec(b).mul(in.y)).add(vec(c).mul(in.z)).add(vec(d));
     693            1 : }
     694              : 
     695            0 : void matrix4::transform(const vec4<float> &in, vec &out) const
     696              : {
     697            0 :     out = vec(a).mul(in.x).add(vec(b).mul(in.y)).add(vec(c).mul(in.z)).add(vec(d).mul(in.w));
     698            0 : }
     699              : 
     700            0 : void matrix4::transform(const vec &in, vec4<float> &out) const
     701              : {
     702            0 :     out = vec4<float>(a).mul(in.x).madd(b, in.y).madd(c, in.z).add(d);
     703            0 : }
     704              : 
     705            0 : void matrix4::transform(const vec4<float> &in, vec4<float> &out) const
     706              : {
     707            0 :     out = vec4<float>(a).mul(in.x).madd(b, in.y).madd(c, in.z).madd(d, in.w);
     708            0 : }
     709              : 
     710            4 : void matrix4::transformnormal(const vec &in, vec &out) const
     711              : {
     712            4 :     out = vec(a).mul(in.x).add(vec(b).mul(in.y)).add(vec(c).mul(in.z));
     713            4 : }
     714              : 
     715            3 : void matrix4::transformnormal(const vec &in, vec4<float> &out) const
     716              : {
     717            3 :     out = vec4<float>(a).mul(in.x).madd(b, in.y).madd(c, in.z);
     718            3 : }
     719              : 
     720            3 : void matrix4::transposedtransform(const vec &in, vec &out) const
     721              : {
     722            3 :     vec p = vec(in).sub(vec(d));
     723            3 :     out.x = a.dot3(p);
     724            3 :     out.y = b.dot3(p);
     725            3 :     out.z = c.dot3(p);
     726            3 : }
     727              : 
     728            3 : void matrix4::transposedtransformnormal(const vec &in, vec &out) const
     729              : {
     730            3 :     out.x = a.dot3(in);
     731            3 :     out.y = b.dot3(in);
     732            3 :     out.z = c.dot3(in);
     733            3 : }
     734              : 
     735            1 : vec matrix4::gettranslation() const
     736              : {
     737            1 :     return vec(d);
     738              : }
     739              : 
     740            2 : vec4<float> matrix4::rowx() const
     741              : {
     742            2 :     return vec4<float>(a.x, b.x, c.x, d.x);
     743              : }
     744              : 
     745            2 : vec4<float> matrix4::rowy() const
     746              : {
     747            2 :     return vec4<float>(a.y, b.y, c.y, d.y);
     748              : }
     749              : 
     750            2 : vec4<float> matrix4::rowz() const
     751              : {
     752            2 :     return vec4<float>(a.z, b.z, c.z, d.z);
     753              : }
     754              : 
     755            2 : vec4<float> matrix4::roww() const
     756              : {
     757            2 :     return vec4<float>(a.w, b.w, c.w, d.w);
     758              : }
     759              : 
     760            3 : vec2 matrix4::lineardepthscale() const
     761              : {
     762            3 :     return vec2(d.w, -d.z).div(c.z*d.w - d.z*c.w);
     763              : }
     764              : 
     765              : // =============================================================================
     766              : //  matrix4x3 object
     767              : // =============================================================================
     768              : 
     769           63 : matrix4x3::matrix4x3() : a(0,0,0), b(0,0,0), c(0,0,0), d(0,0,0)
     770              : {
     771           63 : }
     772              : 
     773           33 : matrix4x3::matrix4x3(const vec &a, const vec &b, const vec &c, const vec &d) : a(a), b(b), c(c), d(d)
     774              : {
     775           33 : }
     776              : 
     777            9 : matrix4x3::matrix4x3(const matrix3 &rot, const vec &trans) : a(rot.a), b(rot.b), c(rot.c), d(trans)
     778              : {
     779            9 : }
     780              : 
     781            2 : matrix4x3::matrix4x3(const dualquat &dq)
     782              : {
     783            2 :     vec4<float> r = vec4<float>(dq.real).mul(1/dq.real.squaredlen()), rr = vec4<float>(r).mul(dq.real);
     784            2 :     r.mul(2);
     785            2 :     float xy = r.x*dq.real.y, xz = r.x*dq.real.z, yz = r.y*dq.real.z,
     786            2 :           wx = r.w*dq.real.x, wy = r.w*dq.real.y, wz = r.w*dq.real.z;
     787            2 :     a = vec(rr.w + rr.x - rr.y - rr.z, xy + wz, xz - wy);
     788            2 :     b = vec(xy - wz, rr.w + rr.y - rr.x - rr.z, yz + wx);
     789            2 :     c = vec(xz + wy, yz - wx, rr.w + rr.z - rr.x - rr.y);
     790            4 :     d = vec(-(dq.dual.w*r.x - dq.dual.x*r.w + dq.dual.y*r.z - dq.dual.z*r.y),
     791            2 :             -(dq.dual.w*r.y - dq.dual.x*r.z - dq.dual.y*r.w + dq.dual.z*r.x),
     792            2 :             -(dq.dual.w*r.z + dq.dual.x*r.y - dq.dual.y*r.x - dq.dual.z*r.w));
     793            2 : }
     794              : 
     795            4 : void matrix4x3::mul(float k)
     796              : {
     797            4 :     a.mul(k);
     798            4 :     b.mul(k);
     799            4 :     c.mul(k);
     800            4 :     d.mul(k);
     801            4 : }
     802              : 
     803            3 : void matrix4x3::setscale(float x, float y, float z)
     804              : {
     805            3 :     a.x = x;
     806            3 :     b.y = y;
     807            3 :     c.z = z;
     808            3 : }
     809              : 
     810            1 : void matrix4x3::setscale(const vec &v)
     811              : {
     812            1 :     setscale(v.x, v.y, v.z);
     813            1 : }
     814              : 
     815            1 : void matrix4x3::setscale(float n)
     816              : {
     817            1 :     setscale(n, n, n);
     818            1 : }
     819              : 
     820            3 : void matrix4x3::scale(float x, float y, float z)
     821              : {
     822            3 :     a.mul(x);
     823            3 :     b.mul(y);
     824            3 :     c.mul(z);
     825            3 : }
     826              : 
     827            1 : void matrix4x3::scale(const vec &v)
     828              : {
     829            1 :     scale(v.x, v.y, v.z);
     830            1 : }
     831              : 
     832            1 : void matrix4x3::scale(float n)
     833              : {
     834            1 :     scale(n, n, n);
     835            1 : }
     836              : 
     837            1 : void matrix4x3::settranslation(const vec &p)
     838              : {
     839            1 :     d = p;
     840            1 : }
     841              : 
     842            1 : void matrix4x3::settranslation(float x, float y, float z)
     843              : {
     844            1 :     d = vec(x, y, z);
     845            1 : }
     846              : 
     847            4 : void matrix4x3::translate(const vec &p)
     848              : {
     849            4 :     d.madd(a, p.x).madd(b, p.y).madd(c, p.z);
     850            4 : }
     851              : 
     852            1 : void matrix4x3::translate(float x, float y, float z)
     853              : {
     854            1 :     translate(vec(x, y, z));
     855            1 : }
     856              : 
     857            3 : void matrix4x3::translate(const vec &p, float scale)
     858              : {
     859            3 :     translate(vec(p).mul(scale));
     860            3 : }
     861              : 
     862            1 : void matrix4x3::accumulate(const matrix4x3 &m, float k)
     863              : {
     864            1 :     a.madd(m.a, k);
     865            1 :     b.madd(m.b, k);
     866            1 :     c.madd(m.c, k);
     867            1 :     d.madd(m.d, k);
     868            1 : }
     869              : 
     870            4 : void matrix4x3::normalize()
     871              : {
     872            4 :     a.normalize();
     873            4 :     b.normalize();
     874            4 :     c.normalize();
     875            4 : }
     876              : 
     877            1 : void matrix4x3::lerp(const matrix4x3 &to, float t)
     878              : {
     879            1 :     a.lerp(to.a, t);
     880            1 :     b.lerp(to.b, t);
     881            1 :     c.lerp(to.c, t);
     882            1 :     d.lerp(to.d, t);
     883            1 : }
     884            1 : void matrix4x3::lerp(const matrix4x3 &from, const matrix4x3 &to, float t)
     885              : {
     886            1 :     a.lerp(from.a, to.a, t);
     887            1 :     b.lerp(from.b, to.b, t);
     888            1 :     c.lerp(from.c, to.c, t);
     889            1 :     d.lerp(from.d, to.d, t);
     890            1 : }
     891              : 
     892           46 : void matrix4x3::identity()
     893              : {
     894           46 :     a = vec(1, 0, 0);
     895           46 :     b = vec(0, 1, 0);
     896           46 :     c = vec(0, 0, 1);
     897           46 :     d = vec(0, 0, 0);
     898           46 : }
     899              : 
     900            2 : void matrix4x3::mul(const matrix4x3 &m, const matrix4x3 &n)
     901              : {
     902            2 :     a = vec(m.a).mul(n.a.x).madd(m.b, n.a.y).madd(m.c, n.a.z);
     903            2 :     b = vec(m.a).mul(n.b.x).madd(m.b, n.b.y).madd(m.c, n.b.z);
     904            2 :     c = vec(m.a).mul(n.c.x).madd(m.b, n.c.y).madd(m.c, n.c.z);
     905            2 :     d = vec(m.d).madd(m.a, n.d.x).madd(m.b, n.d.y).madd(m.c, n.d.z);
     906            2 : }
     907              : 
     908            1 : void matrix4x3::mul(const matrix4x3 &n)
     909              : {
     910            1 :     mul(matrix4x3(*this), n);
     911            1 : }
     912              : 
     913            1 : void matrix4x3::mul(const matrix3 &m, const matrix4x3 &n)
     914              : {
     915            1 :     a = vec(m.a).mul(n.a.x).madd(m.b, n.a.y).madd(m.c, n.a.z);
     916            1 :     b = vec(m.a).mul(n.b.x).madd(m.b, n.b.y).madd(m.c, n.b.z);
     917            1 :     c = vec(m.a).mul(n.c.x).madd(m.b, n.c.y).madd(m.c, n.c.z);
     918            1 :     d = vec(m.a).mul(n.d.x).madd(m.b, n.d.y).madd(m.c, n.d.z);
     919            1 : }
     920              : 
     921            1 : void matrix4x3::mul(const matrix3 &rot, const vec &trans, const matrix4x3 &n)
     922              : {
     923            1 :     mul(rot, n);
     924            1 :     d.add(trans);
     925            1 : }
     926              : 
     927            3 : void matrix4x3::transpose()
     928              : {
     929            3 :     d = vec(a.dot(d), b.dot(d), c.dot(d)).neg();
     930            3 :     std::swap(a.y, b.x);
     931            3 :     std::swap(a.z, c.x);
     932            3 :     std::swap(b.z, c.y);
     933            3 : }
     934              : 
     935            6 : void matrix4x3::transpose(const matrix4x3 &o)
     936              : {
     937            6 :     a = vec(o.a.x, o.b.x, o.c.x);
     938            6 :     b = vec(o.a.y, o.b.y, o.c.y);
     939            6 :     c = vec(o.a.z, o.b.z, o.c.z);
     940            6 :     d = vec(o.a.dot(o.d), o.b.dot(o.d), o.c.dot(o.d)).neg();
     941            6 : }
     942              : 
     943            2 : void matrix4x3::transposemul(const matrix4x3 &m, const matrix4x3 &n)
     944              : {
     945            2 :     vec t(m.a.dot(m.d), m.b.dot(m.d), m.c.dot(m.d));
     946            2 :     a = vec(m.a.dot(n.a), m.b.dot(n.a), m.c.dot(n.a));
     947            2 :     b = vec(m.a.dot(n.b), m.b.dot(n.b), m.c.dot(n.b));
     948            2 :     c = vec(m.a.dot(n.c), m.b.dot(n.c), m.c.dot(n.c));
     949            2 :     d = vec(m.a.dot(n.d), m.b.dot(n.d), m.c.dot(n.d)).sub(t);
     950            2 : }
     951              : 
     952            1 : void matrix4x3::multranspose(const matrix4x3 &m, const matrix4x3 &n)
     953              : {
     954            1 :     vec t(n.a.dot(n.d), n.b.dot(n.d), n.c.dot(n.d));
     955            1 :     a = vec(m.a).mul(n.a.x).madd(m.b, n.b.x).madd(m.c, n.c.x);
     956            1 :     b = vec(m.a).mul(n.a.y).madd(m.b, m.b.y).madd(m.c, n.c.y);
     957            1 :     c = vec(m.a).mul(n.a.z).madd(m.b, n.b.z).madd(m.c, n.c.z);
     958            1 :     d = vec(m.d).msub(m.a, t.x).msub(m.b, t.y).msub(m.c, t.z);
     959            1 : }
     960              : 
     961            6 : void matrix4x3::invert(const matrix4x3 &o)
     962              : {
     963            6 :     vec unscale(1/o.a.squaredlen(), 1/o.b.squaredlen(), 1/o.c.squaredlen());
     964            6 :     transpose(o);
     965            6 :     a.mul(unscale);
     966            6 :     b.mul(unscale);
     967            6 :     c.mul(unscale);
     968            6 :     d.mul(unscale);
     969            6 : }
     970              : 
     971            4 : void matrix4x3::invert()
     972              : {
     973            4 :     invert(matrix4x3(*this));
     974            4 : }
     975              : 
     976            3 : void matrix4x3::rotate(float angle, const vec &d)
     977              : {
     978            3 :     rotate(cosf(angle), std::sin(angle), d);
     979            3 : }
     980              : 
     981            6 : void matrix4x3::rotate(float ck, float sk, const vec &axis)
     982              : {
     983            6 :     matrix3 m;
     984            6 :     m.rotate(ck, sk, axis);
     985            6 :     *this = matrix4x3(m, vec(0, 0, 0));
     986            6 : }
     987              : 
     988            2 : void matrix4x3::rotate_around_x(float ck, float sk)
     989              : {
     990            2 :     vec rb = vec(b).mul(ck).madd(c, sk),
     991            2 :         rc = vec(c).mul(ck).msub(b, sk);
     992            2 :     b = rb;
     993            2 :     c = rc;
     994            2 : }
     995            1 : void matrix4x3::rotate_around_x(float angle)
     996              : {
     997            1 :     rotate_around_x(cosf(angle), std::sin(angle));
     998            1 : }
     999              : 
    1000            1 : void matrix4x3::rotate_around_x(const vec2 &sc)
    1001              : {
    1002            1 :     rotate_around_x(sc.x, sc.y);
    1003            1 : }
    1004              : 
    1005            2 : void matrix4x3::rotate_around_y(float ck, float sk)
    1006              : {
    1007            2 :     vec rc = vec(c).mul(ck).madd(a, sk),
    1008            2 :         ra = vec(a).mul(ck).msub(c, sk);
    1009            2 :     c = rc;
    1010            2 :     a = ra;
    1011            2 : }
    1012            1 : void matrix4x3::rotate_around_y(float angle)
    1013              : {
    1014            1 :     rotate_around_y(cosf(angle), std::sin(angle));
    1015            1 : }
    1016              : 
    1017            1 : void matrix4x3::rotate_around_y(const vec2 &sc)
    1018              : {
    1019            1 :     rotate_around_y(sc.x, sc.y);
    1020            1 : }
    1021              : 
    1022            2 : void matrix4x3::rotate_around_z(float ck, float sk)
    1023              : {
    1024            2 :     vec ra = vec(a).mul(ck).madd(b, sk),
    1025            2 :         rb = vec(b).mul(ck).msub(a, sk);
    1026            2 :     a = ra;
    1027            2 :     b = rb;
    1028            2 : }
    1029              : 
    1030            1 : void matrix4x3::rotate_around_z(float angle)
    1031              : {
    1032            1 :     rotate_around_z(cosf(angle), std::sin(angle));
    1033            1 : }
    1034              : 
    1035            1 : void matrix4x3::rotate_around_z(const vec2 &sc)
    1036              : {
    1037            1 :     rotate_around_z(sc.x, sc.y);
    1038            1 : }
    1039              : 
    1040            3 : vec matrix4x3::transposedtransform(const vec &o) const
    1041              : {
    1042            3 :     vec p = vec(o).sub(d);
    1043            3 :     return vec(a.dot(p), b.dot(p), c.dot(p));
    1044              : }
    1045              : 
    1046            3 : vec matrix4x3::transformnormal(const vec &o) const
    1047              : {
    1048            3 :     return vec(a).mul(o.x).madd(b, o.y).madd(c, o.z);
    1049              : }
    1050              : 
    1051            3 : vec matrix4x3::transposedtransformnormal(const vec &o) const
    1052              : {
    1053            3 :     return vec(a.dot(o), b.dot(o), c.dot(o));
    1054              : }
    1055              : 
    1056           12 : vec matrix4x3::transform(const vec &o) const
    1057              : {
    1058           12 :     return vec(d).madd(a, o.x).madd(b, o.y).madd(c, o.z);
    1059              : }
    1060              : 
    1061            3 : vec matrix4x3::transform(const vec2 &o) const
    1062              : {
    1063            3 :     return vec(d).madd(a, o.x).madd(b, o.y);
    1064              : }
    1065              : 
    1066            1 : vec4<float> matrix4x3::rowx() const
    1067              : {
    1068            1 :     return vec4<float>(a.x, b.x, c.x, d.x);
    1069              : }
    1070              : 
    1071            1 : vec4<float> matrix4x3::rowy() const
    1072              : {
    1073            1 :     return vec4<float>(a.y, b.y, c.y, d.y);
    1074              : }
    1075              : 
    1076            1 : vec4<float> matrix4x3::rowz() const
    1077              : {
    1078            1 :     return vec4<float>(a.z, b.z, c.z, d.z);
    1079              : }
    1080              : 
    1081              : //end matrix4x3
        

Generated by: LCOV version 2.0-1