Changeset 255 for cpp/frams/util


Ignore:
Timestamp:
11/18/14 17:03:27 (9 years ago)
Author:
Maciej Komosinski
Message:

Formatted source and new Orient::lookAt() function with only one vector argument

Location:
cpp/frams/util
Files:
2 edited

Legend:

Unmodified
Added
Removed
  • cpp/frams/util/3d.cpp

    r197 r255  
    77#include "3d.h"
    88
    9 Pt3D operator+(const Pt3D &p1,const Pt3D &p2) {return Pt3D(p1.x+p2.x,p1.y+p2.y,p1.z+p2.z);}
    10 Pt3D operator-(const Pt3D &p1,const Pt3D &p2) {return Pt3D(p1.x-p2.x,p1.y-p2.y,p1.z-p2.z);}
    11 
    12 Pt3D Pt3D_0(0,0,0);
    13 
    14 bool Pt3D::report_errors=true;
     9Pt3D operator+(const Pt3D &p1, const Pt3D &p2) { return Pt3D(p1.x + p2.x, p1.y + p2.y, p1.z + p2.z); }
     10Pt3D operator-(const Pt3D &p1, const Pt3D &p2) { return Pt3D(p1.x - p2.x, p1.y - p2.y, p1.z - p2.z); }
     11
     12Pt3D Pt3D_0(0, 0, 0);
     13
     14bool Pt3D::report_errors = true;
    1515
    1616double Pt3D::operator()() const
    1717{
    18 double q=x*x+y*y+z*z;
    19 if (q<0) {if (report_errors) FramMessage("Pt3D","operator()","sqrt domain error",3); return 0;}
    20 return sqrt(q);
     18        double q = x*x + y*y + z*z;
     19        if (q < 0) { if (report_errors) FramMessage("Pt3D", "operator()", "sqrt domain error", 3); return 0; }
     20        return sqrt(q);
    2121}
    2222
    2323bool Pt3D::normalize()
    2424{
    25 double len=length();
    26 if (fabs(len)<1e-50) {if (report_errors) FramMessage("Pt3D","normalize()","vector too small",1); x=1;y=0;z=0; return false;}
    27 operator/=(len);
    28 return true;
     25        double len = length();
     26        if (fabs(len) < 1e-50) { if (report_errors) FramMessage("Pt3D", "normalize()", "vector too small", 1); x = 1; y = 0; z = 0; return false; }
     27        operator/=(len);
     28        return true;
    2929}
    3030
    3131double Pt3D::distanceTo(const Pt3D& p) const
    3232{
    33 return sqrt((x-p.x)*(x-p.x)+(y-p.y)*(y-p.y)+(z-p.z)*(z-p.z));
     33        return sqrt((x - p.x)*(x - p.x) + (y - p.y)*(y - p.y) + (z - p.z)*(z - p.z));
    3434}
    3535
    3636double Pt3D::manhattanDistanceTo(const Pt3D& p) const
    3737{
    38 return fabs(x-p.x)+fabs(y-p.y)+fabs(z-p.z);
    39 }
    40 
    41 Orient Orient_1(Pt3D(1,0,0),Pt3D(0,1,0),Pt3D(0,0,1));
     38        return fabs(x - p.x) + fabs(y - p.y) + fabs(z - p.z);
     39}
     40
     41Orient Orient_1(Pt3D(1, 0, 0), Pt3D(0, 1, 0), Pt3D(0, 0, 1));
    4242
    4343// prosty obrot
    44 void rotate2D(double k,double &x,double &y)
    45 {double s=sin(k),c=cos(k);
    46 double t=c*x-s*y; y=s*x+c*y; x=t;}
    47 
    48 void rotate2D(double s,double c,double &x,double &y)
    49 {double t=c*x-s*y; y=s*x+c*y; x=t;}
    50 
    51 int Pt3D::getAngle(double dx,double dy,double &wyn)
    52 {
    53 if ((fabs(dx)+fabs(dy))<0.0001) return 0;
    54 wyn=atan2(dy,dx);
    55 return 1;
    56 }
    57 
    58 void Pt3D::getAngles(const Pt3D& X,const Pt3D& dir)
    59 {
    60 Pt3D    t1(X), t2(dir);
    61 if (getAngle(t1.x,t1.y,z))
    62         {       // nie pionowo
    63         rotate2D(-z,t1.x,t1.y);
    64         rotate2D(-z,t2.x,t2.y);
    65         getAngle(t1.x, t1.z, y);
    66         }
    67 else
    68         {       // pionowo
    69         z=0;
    70         if (t1.z<0)
    71                 y=-M_PI_2; // w dol
    72         else
    73                 y=M_PI_2; // w gore
    74         }
    75 rotate2D(-y,t2.x,t2.z);
    76 if (!getAngle(t2.z,-t2.y,x))
    77         x=0;// to zly wynik ale dobrego nie ma :P
     44void rotate2D(double k, double &x, double &y)
     45{
     46        double s = sin(k), c = cos(k);
     47        double t = c*x - s*y; y = s*x + c*y; x = t;
     48}
     49
     50void rotate2D(double s, double c, double &x, double &y)
     51{
     52        double t = c*x - s*y; y = s*x + c*y; x = t;
     53}
     54
     55int Pt3D::getAngle(double dx, double dy, double &wyn)
     56{
     57        if ((fabs(dx) + fabs(dy)) < 0.0001) return 0;
     58        wyn = atan2(dy, dx);
     59        return 1;
     60}
     61
     62void Pt3D::getAngles(const Pt3D& X, const Pt3D& dir)
     63{
     64        Pt3D    t1(X), t2(dir);
     65        if (getAngle(t1.x, t1.y, z)) // non-vertical
     66        {       
     67                rotate2D(-z, t1.x, t1.y);
     68                rotate2D(-z, t2.x, t2.y);
     69                getAngle(t1.x, t1.z, y);
     70        }
     71        else // vertical
     72        {       
     73                z = 0;
     74                if (t1.z < 0)
     75                        y = -M_PI_2; // down
     76                else
     77                        y = M_PI_2; // up
     78        }
     79        rotate2D(-y, t2.x, t2.z);
     80        if (!getAngle(t2.z, -t2.y, x))
     81                x = 0; // incorrect result, but there is no correct one
    7882}
    7983
    8084void Pt3D::getMin(const Pt3D& p)
    8185{
    82 if (p.x<x) x=p.x;
    83 if (p.y<y) y=p.y;
    84 if (p.z<z) z=p.z;
     86        if (p.x < x) x = p.x;
     87        if (p.y < y) y = p.y;
     88        if (p.z < z) z = p.z;
    8589}
    8690void Pt3D::getMax(const Pt3D& p)
    8791{
    88 if (p.x>x) x=p.x;
    89 if (p.y>y) y=p.y;
    90 if (p.z>z) z=p.z;
    91 }
    92 
    93 void Pt3D::vectorProduct(const Pt3D& a,const Pt3D& b)
    94 {
    95 x=a.y*b.z-a.z*b.y;
    96 y=a.z*b.x-a.x*b.z;
    97 z=a.x*b.y-a.y*b.x;
    98 }
    99 
    100 void Orient::lookAt(const Pt3D& X,const Pt3D& dir)
    101 {
    102 x=X; x.normalize();
    103 y.vectorProduct(dir,x);
    104 z.vectorProduct(x,y);
    105 if ((!y.normalize()) || (!z.normalize()))
    106         {
    107         y.x=dir.y; y.y=dir.z; y.z=dir.x;
    108         z.vectorProduct(x,y);
    109         z.normalize();
    110         }
    111 }
    112 
    113 // odleglosc 2d
    114 double d2(double x,double y)
    115 {
    116 double q=x*x+y*y;
    117 if (q<0) {if (Pt3D::report_errors) FramMessage("","d2()","sqrt domain error",3); return 0;}
    118 return sqrt(q);
     92        if (p.x > x) x = p.x;
     93        if (p.y > y) y = p.y;
     94        if (p.z > z) z = p.z;
     95}
     96
     97void Pt3D::vectorProduct(const Pt3D& a, const Pt3D& b)
     98{
     99        x = a.y*b.z - a.z*b.y;
     100        y = a.z*b.x - a.x*b.z;
     101        z = a.x*b.y - a.y*b.x;
     102}
     103
     104void Orient::lookAt(const Pt3D& X, const Pt3D& dir)
     105{
     106        x = X; x.normalize();
     107        y.vectorProduct(dir, x);
     108        z.vectorProduct(x, y);
     109        if ((!y.normalize()) || (!z.normalize()))
     110                lookAt(X);// dir was (nearly?) parallel, there is no good solution, use the x-only variant
     111}
     112
     113void Orient::lookAt(const Pt3D& X)
     114{
     115        x = X; x.normalize();
     116        // "invent" y vector, not parallel to x
     117        double ax = fabs(x.x), ay = fabs(x.y), az = fabs(x.z);
     118        // find the smallest component
     119        if ((ax <= ay) && (ax <= az)) // x
     120        {
     121                y.x = 0; y.y = -x.z; y.z = x.y; // (0,-z,y)
     122        }
     123        if ((ay <= ax) && (ay <= az)) // y
     124        {
     125                y.x = -x.z; y.y = 0; y.z = x.x; // (-z,0,x)
     126        }
     127        else // z
     128        {
     129                y.x = -x.y; y.y = x.x; y.z = 0; // (-y,x,0)
     130        }
     131        y.normalize();
     132        z.vectorProduct(x, y);
     133}
     134
     135// 2D distance
     136double d2(double x, double y)
     137{
     138        double q = x*x + y*y;
     139        if (q < 0) { if (Pt3D::report_errors) FramMessage("", "d2()", "sqrt domain error", 3); return 0; }
     140        return sqrt(q);
    119141}
    120142
    121143Orient::Orient(const Matrix44& m)
    122144{
    123 x.x=m[0];  x.y=m[1];  x.z=m[2];
    124 y.x=m[4];  y.y=m[5];  y.z=m[6];
    125 z.x=m[8];  z.y=m[9];  z.z=m[10];
     145        x.x = m[0];  x.y = m[1];  x.z = m[2];
     146        y.x = m[4];  y.y = m[5];  y.z = m[6];
     147        z.x = m[8];  z.y = m[9];  z.z = m[10];
    126148}
    127149
    128150void Orient::operator=(const Pt3D &rot)
    129151{
    130 *this=Orient_1;
    131 rotate(rot);
     152        *this = Orient_1;
     153        rotate(rot);
    132154}
    133155
    134156void Orient::rotate(const Pt3D &v)
    135157{
    136 double s,c;
    137 if (fabs(v.x)>0.0001)
    138         {
    139         s=sin(v.x); c=cos(v.x);
    140         rotate2D(s,c,x.y,x.z);
    141         rotate2D(s,c,y.y,y.z);
    142         rotate2D(s,c,z.y,z.z);
    143         }
    144 if (fabs(v.y)>0.0001)
    145         {
    146         s=sin(v.y); c=cos(v.y);
    147         rotate2D(s,c,x.x,x.z);
    148         rotate2D(s,c,y.x,y.z);
    149         rotate2D(s,c,z.x,z.z);
    150         }
    151 if (fabs(v.z)>0.0001)
    152         {
    153         s=sin(v.z); c=cos(v.z);
    154         rotate2D(s,c,x.x,x.y);
    155         rotate2D(s,c,y.x,y.y);
    156         rotate2D(s,c,z.x,z.y);
    157         }
    158 }
    159 
    160 void Orient::transform(Pt3D& target,const Pt3D &s) const
    161 {
    162 target.x=s.x*x.x+s.y*y.x+s.z*z.x;
    163 target.y=s.x*x.y+s.y*y.y+s.z*z.y;
    164 target.z=s.x*x.z+s.y*y.z+s.z*z.z;
    165 }
    166 
    167 void Orient::revTransform(Pt3D& target,const Pt3D &s) const
    168 {
    169 target.x=s.x*x.x+s.y*x.y+s.z*x.z;
    170 target.y=s.x*y.x+s.y*y.y+s.z*y.z;
    171 target.z=s.x*z.x+s.y*z.y+s.z*z.z;
    172 }
    173 
    174 void Orient::transform(Orient& target,const Orient& src) const
    175 {
    176 transform(target.x,src.x);
    177 transform(target.y,src.y);
    178 transform(target.z,src.z);
    179 }
    180 
    181 void Orient::revTransform(Orient& target,const Orient& src) const
    182 {
    183 revTransform(target.x,src.x);
    184 revTransform(target.y,src.y);
    185 revTransform(target.z,src.z);
     158        double s, c;
     159        if (fabs(v.x) > 0.0001)
     160        {
     161                s = sin(v.x); c = cos(v.x);
     162                rotate2D(s, c, x.y, x.z);
     163                rotate2D(s, c, y.y, y.z);
     164                rotate2D(s, c, z.y, z.z);
     165        }
     166        if (fabs(v.y) > 0.0001)
     167        {
     168                s = sin(v.y); c = cos(v.y);
     169                rotate2D(s, c, x.x, x.z);
     170                rotate2D(s, c, y.x, y.z);
     171                rotate2D(s, c, z.x, z.z);
     172        }
     173        if (fabs(v.z) > 0.0001)
     174        {
     175                s = sin(v.z); c = cos(v.z);
     176                rotate2D(s, c, x.x, x.y);
     177                rotate2D(s, c, y.x, y.y);
     178                rotate2D(s, c, z.x, z.y);
     179        }
     180}
     181
     182void Orient::transform(Pt3D& target, const Pt3D &s) const
     183{
     184        target.x = s.x*x.x + s.y*y.x + s.z*z.x;
     185        target.y = s.x*x.y + s.y*y.y + s.z*z.y;
     186        target.z = s.x*x.z + s.y*y.z + s.z*z.z;
     187}
     188
     189void Orient::revTransform(Pt3D& target, const Pt3D &s) const
     190{
     191        target.x = s.x*x.x + s.y*x.y + s.z*x.z;
     192        target.y = s.x*y.x + s.y*y.y + s.z*y.z;
     193        target.z = s.x*z.x + s.y*z.y + s.z*z.z;
     194}
     195
     196void Orient::transform(Orient& target, const Orient& src) const
     197{
     198        transform(target.x, src.x);
     199        transform(target.y, src.y);
     200        transform(target.z, src.z);
     201}
     202
     203void Orient::revTransform(Orient& target, const Orient& src) const
     204{
     205        revTransform(target.x, src.x);
     206        revTransform(target.y, src.y);
     207        revTransform(target.z, src.z);
    186208}
    187209
    188210void Orient::getAngles(Pt3D &angles) const
    189211{
    190 angles.getAngles(x,z);
     212        angles.getAngles(x, z);
    191213}
    192214
    193215bool Orient::normalize()
    194216{
    195 bool ret=1;
    196 y.vectorProduct(z,x);
    197 z.vectorProduct(x,y);
    198 if (!x.normalize()) ret=0;
    199 if (!z.normalize()) ret=0;
    200 if (!y.normalize()) ret=0;
    201 return ret;
     217        bool ret = 1;
     218        y.vectorProduct(z, x);
     219        z.vectorProduct(x, y);
     220        if (!x.normalize()) ret = 0;
     221        if (!z.normalize()) ret = 0;
     222        if (!y.normalize()) ret = 0;
     223        return ret;
    202224}
    203225
    204226Matrix44::Matrix44(const Orient &rot)
    205227{
    206 m[0]=rot.x.x;  m[1]=rot.x.y;  m[2]=rot.x.z;  m[3]=0;
    207 m[4]=rot.y.x;  m[5]=rot.y.y;  m[6]=rot.y.z;  m[7]=0;
    208 m[8]=rot.z.x;  m[9]=rot.z.y;  m[10]=rot.z.z; m[11]=0;
    209 m[12]=0;       m[13]=0;       m[14]=0;       m[15]=1;
     228        m[0] = rot.x.x;  m[1] = rot.x.y;  m[2] = rot.x.z;  m[3] = 0;
     229        m[4] = rot.y.x;  m[5] = rot.y.y;  m[6] = rot.y.z;  m[7] = 0;
     230        m[8] = rot.z.x;  m[9] = rot.z.y;  m[10] = rot.z.z; m[11] = 0;
     231        m[12] = 0;       m[13] = 0;       m[14] = 0;       m[15] = 1;
    210232}
    211233
  • cpp/frams/util/3d.h

    r225 r255  
    1515\file 3d.h 3d.cpp
    1616
    17    basic 3d classes and operators     
     17basic 3D classes and operators
    1818*********************************/
    1919
    20 /// point in 3d space
    21 
     20/// point in 3D space
    2221class Pt3D
    2322{
    24 public: double x,y,z;
    25 static bool report_errors;
     23public:
     24        double x, y, z;
     25        static bool report_errors;
    2626
    27 Pt3D(double _x,double _y,double _z):x(_x),y(_y),z(_z) {} ///< constructor initializing all coords
    28 Pt3D(double xyz):x(xyz),y(xyz),z(xyz) {} ///< all coords equal
    29 Pt3D() {} ///< coords will be not initialized!
    30 Pt3D(const Pt3D &p):x(p.x),y(p.y),z(p.z) {} ///< copy from another point
    31 bool    operator==(const Pt3D& p)       {return (x==p.x)&&(y==p.y)&&(z==p.z);}
    32 void    operator+=(const Pt3D& p)       {x+=p.x;y+=p.y;z+=p.z;}
    33 void    operator-=(const Pt3D& p)       {x-=p.x;y-=p.y;z-=p.z;}
    34 void    operator*=(double d)    {x*=d;y*=d;z*=d;}
    35 Pt3D    operator*(const Pt3D &p) const {return Pt3D(y*p.z-z*p.y, z*p.x-x*p.z, x*p.y-y*p.x);}
    36 void    operator/=(double d)    {x/=d; y/=d; z/=d;}
    37 //Pt3D  operator+(const Pt3D& p) const {return Pt3D(x+p.x,y+p.y,z+p.z);}
    38 //Pt3D  operator-(const Pt3D& p) const {return Pt3D(x-p.x,y-p.y,z-p.z);}
    39 Pt3D    operator-() const {return Pt3D(-x,-y,-z);}
    40 Pt3D    operator*(double d) const {return Pt3D(x*d,y*d,z*d);}
    41 Pt3D    operator/(double d) const {return Pt3D(x/d,y/d,z/d);}
    42 bool    allCoordsLowerThan(const Pt3D& p) const {return (x<p.x)&&(y<p.y)&&(z<p.z);}
    43 bool    allCoordsHigherThan(const Pt3D& p) const {return (x>p.x)&&(y>p.y)&&(z>p.z);}
    44 void getMin(const Pt3D& p);
    45 void getMax(const Pt3D& p);
    46 /** vector length = \f$\sqrt{x^2+y^2+z^2}\f$  */
    47 double operator()() const;
    48 /** vector length = \f$\sqrt{x^2+y^2+z^2}\f$  */
    49 double length() const {return operator()();}
    50 double length2() const {return x*x+y*y+z*z;}
    51 double distanceTo(const Pt3D& p) const;
    52 double manhattanDistanceTo(const Pt3D& p) const;
    53 /** calculate angle between (0,0)-(dx,dy), @return 1=ok, 0=can't calculate */
    54 static int getAngle(double dx,double dy,double &angle);
    55 /** calculate 3 rotation angles translating (1,0,0) into 'X' and (0,0,1) into 'dir' */
    56 void getAngles(const Pt3D& X,const Pt3D& dir);
    57 void vectorProduct(const Pt3D& a,const Pt3D& b);
    58 Pt3D vectorProduct(const Pt3D& p) const {return (*this)*p;}
    59 Pt3D entrywiseProduct(const Pt3D &p) const {return Pt3D(x*p.x,y*p.y,z*p.z);} ///< also known as Hadamard product or Schur product
    60 double dotProduct(const Pt3D& p) const {return x*p.x+y*p.y+z*p.z;}
    61 bool normalize();
     27        Pt3D(double _x, double _y, double _z) :x(_x), y(_y), z(_z) {} ///< constructor initializing all coords
     28        Pt3D(double xyz) :x(xyz), y(xyz), z(xyz) {} ///< all coords equal
     29        Pt3D() {} ///< coords will be not initialized!
     30        Pt3D(const Pt3D &p) :x(p.x), y(p.y), z(p.z) {} ///< copy from another point
     31        bool    operator==(const Pt3D& p)       { return (x == p.x) && (y == p.y) && (z == p.z); }
     32        void    operator+=(const Pt3D& p)       { x += p.x; y += p.y; z += p.z; }
     33        void    operator-=(const Pt3D& p)       { x -= p.x; y -= p.y; z -= p.z; }
     34        void    operator*=(double d)    { x *= d; y *= d; z *= d; }
     35        Pt3D    operator*(const Pt3D &p) const { return Pt3D(y*p.z - z*p.y, z*p.x - x*p.z, x*p.y - y*p.x); }
     36        void    operator/=(double d)    { x /= d; y /= d; z /= d; }
     37        //Pt3D  operator+(const Pt3D& p) const {return Pt3D(x+p.x,y+p.y,z+p.z);}
     38        //Pt3D  operator-(const Pt3D& p) const {return Pt3D(x-p.x,y-p.y,z-p.z);}
     39        Pt3D    operator-() const { return Pt3D(-x, -y, -z); }
     40        Pt3D    operator*(double d) const { return Pt3D(x*d, y*d, z*d); }
     41        Pt3D    operator/(double d) const { return Pt3D(x / d, y / d, z / d); }
     42        bool    allCoordsLowerThan(const Pt3D& p) const { return (x < p.x) && (y < p.y) && (z<p.z); }
     43        bool    allCoordsHigherThan(const Pt3D& p) const { return (x>p.x) && (y > p.y) && (z > p.z); }
     44        void getMin(const Pt3D& p);
     45        void getMax(const Pt3D& p);
     46        /** vector length = \f$\sqrt{x^2+y^2+z^2}\f$  */
     47        double operator()() const;
     48        /** vector length = \f$\sqrt{x^2+y^2+z^2}\f$  */
     49        double length() const { return operator()(); }
     50        double length2() const { return x*x + y*y + z*z; }
     51        double distanceTo(const Pt3D& p) const;
     52        double manhattanDistanceTo(const Pt3D& p) const;
     53        /** calculate angle between (0,0)-(dx,dy), @return 1=ok, 0=can't calculate */
     54        static int getAngle(double dx, double dy, double &angle);
     55        /** calculate 3 rotation angles translating (1,0,0) into 'X' and (0,0,1) into 'dir' */
     56        void getAngles(const Pt3D& X, const Pt3D& dir);
     57        void vectorProduct(const Pt3D& a, const Pt3D& b);
     58        Pt3D vectorProduct(const Pt3D& p) const { return (*this)*p; }
     59        Pt3D entrywiseProduct(const Pt3D &p) const { return Pt3D(x*p.x, y*p.y, z*p.z); } ///< also known as Hadamard product or Schur product
     60        double dotProduct(const Pt3D& p) const { return x*p.x + y*p.y + z*p.z; }
     61        bool normalize();
    6262};
    63 Pt3D operator+(const Pt3D &p1,const Pt3D &p2);
    64 Pt3D operator-(const Pt3D &p1,const Pt3D &p2);
     63
     64Pt3D operator+(const Pt3D &p1, const Pt3D &p2);
     65Pt3D operator-(const Pt3D &p1, const Pt3D &p2);
    6566
    6667class Pt3D_DontReportErrors
    6768{
    68 bool state;
     69        bool state;
    6970public:
    70 Pt3D_DontReportErrors() {state=Pt3D::report_errors; Pt3D::report_errors=false;}
    71 ~Pt3D_DontReportErrors() {Pt3D::report_errors=state;}
     71        Pt3D_DontReportErrors() { state = Pt3D::report_errors; Pt3D::report_errors = false; }
     72        ~Pt3D_DontReportErrors() { Pt3D::report_errors = state; }
    7273};
    7374
    74 ///  orientation in 3d space = rotation matrix
    75 
     75///  orientation in 3D space = rotation matrix
    7676class Matrix44;
    7777
    7878class Orient
    7979{
    80 public: Pt3D x,y,z; ///< 3 vectors (= 3x3 matrix)
     80public:
     81        Pt3D x, y, z; ///< 3 vectors (= 3x3 matrix)
    8182
    8283        Orient() {}
    83         Orient(const Orient& src) {x=src.x; y=src.y; z=src.z;}
    84         Orient(const Pt3D& a,const Pt3D& b,const Pt3D& c):x(a),y(b),z(c) {}
    85 //      Orient(const Pt3D& rot) {*this=rot;}
     84        Orient(const Orient& src) { x = src.x; y = src.y; z = src.z; }
     85        Orient(const Pt3D& a, const Pt3D& b, const Pt3D& c) :x(a), y(b), z(c) {}
     86        //      Orient(const Pt3D& rot) {*this=rot;}
    8687        Orient(const Matrix44& m);
    8788        void operator=(const Pt3D &rot);
    8889        void rotate(const Pt3D &); ///< rotate matrix around 3 axes
    8990
    90         void transform(Pt3D &target,const Pt3D &src) const;     ///< transform a vector
    91         void revTransform(Pt3D &target,const Pt3D &src) const;  ///< reverse transform
    92         Pt3D transform(const Pt3D &src) const {Pt3D t; transform(t,src); return t;}
    93         Pt3D revTransform(const Pt3D &src) const {Pt3D t; revTransform(t,src); return t;}
     91        void transform(Pt3D &target, const Pt3D &src) const;    ///< transform a vector
     92        void revTransform(Pt3D &target, const Pt3D &src) const; ///< reverse transform
     93        Pt3D transform(const Pt3D &src) const { Pt3D t; transform(t, src); return t; }
     94        Pt3D revTransform(const Pt3D &src) const { Pt3D t; revTransform(t, src); return t; }
    9495
    95         void transform(Orient& target,const Orient& src) const;    ///< transform other orient
    96         void revTransform(Orient& target,const Orient& src) const; ///< reverse transform other orient
    97         Orient transform(const Orient& src) const {Orient o; transform(o,src); return o;}    ///< transform other orient
    98         Orient revTransform(const Orient& src) const {Orient o; revTransform(o,src); return o;} ///< reverse transform other orient
     96        void transform(Orient& target, const Orient& src) const;    ///< transform other orient
     97        void revTransform(Orient& target, const Orient& src) const; ///< reverse transform other orient
     98        Orient transform(const Orient& src) const { Orient o; transform(o, src); return o; }    ///< transform other orient
     99        Orient revTransform(const Orient& src) const { Orient o; revTransform(o, src); return o; } ///< reverse transform other orient
    99100
    100         void transformSelf(const Orient &rot) {Orient tmp; rot.transform(tmp,*this); *this=tmp;}
    101         void revTransformSelf(const Orient &rot) {Orient tmp; rot.revTransform(tmp,*this); *this=tmp;}
     101        void transformSelf(const Orient &rot) { Orient tmp; rot.transform(tmp, *this); *this = tmp; }
     102        void revTransformSelf(const Orient &rot) { Orient tmp; rot.revTransform(tmp, *this); *this = tmp; }
    102103
    103104        void getAngles(Pt3D &) const; ///< calculate rotation from current matrix
    104         Pt3D getAngles() const {Pt3D ret; getAngles(ret); return ret;}; ///< calculate rotation from current matrix
    105         void lookAt(const Pt3D &X,const Pt3D &dir); ///< calculate orientation matrix from 2 vectors
     105        Pt3D getAngles() const { Pt3D ret; getAngles(ret); return ret; }; ///< calculate rotation from current matrix
     106        void lookAt(const Pt3D &X, const Pt3D &dir); ///< calculate orientation matrix from 2 vectors: X becomes (normalized) Orient.x, dir is the preferred "up" direction (Orient.z). Use lookAt(Pt3D) if only X is relevant.
     107        void lookAt(const Pt3D &X); ///< calculate orientation matrix from 1 vector, X becomes (normalized) Orient.x, the other coordinates are deterministic but not continuous. Use lookAt(Pt3D,Pt3D) if you need more control.
    106108
    107109        bool normalize();
     
    111113{
    112114public:
    113 double m[16];
    114 Matrix44() {}
    115 Matrix44(const Matrix44& src) {memcpy(m,src.m,sizeof(m));}
    116 Matrix44(double *srcm) {memcpy(m,srcm,sizeof(m));}
    117 Matrix44(const Orient &rot);
     115        double m[16];
     116        Matrix44() {}
     117        Matrix44(const Matrix44& src) { memcpy(m, src.m, sizeof(m)); }
     118        Matrix44(double *srcm) { memcpy(m, srcm, sizeof(m)); }
     119        Matrix44(const Orient &rot);
    118120
    119 const double& operator()(int i,int j) const {return m[i+16*j];}
    120 const double& operator[](int i) const {return m[i];}
    121 double& operator()(int i,int j) {return m[i+16*j];}
    122 double& operator[](int i) {return m[i];}
     121        const double& operator()(int i, int j) const { return m[i + 16 * j]; }
     122        const double& operator[](int i) const { return m[i]; }
     123        double& operator()(int i, int j) { return m[i + 16 * j]; }
     124        double& operator[](int i) { return m[i]; }
    123125
    124 void operator+=(const Pt3D &); ///< translate matrix
    125 void operator*=(const Pt3D &); ///< scale matrix
    126 void operator*=(double sc); ///< scale matrix
     126        void operator+=(const Pt3D &); ///< translate matrix
     127        void operator*=(const Pt3D &); ///< scale matrix
     128        void operator*=(double sc); ///< scale matrix
    127129};
    128130
     
    131133extern Matrix44 Matrix44_1; ///< standard unit matrix: 1000 0100 0010 0001
    132134
    133 void rotate2D(double,double &,double &); ///< rotate 2d vector, given angle
    134 void rotate2D(double,double,double &,double &); ///< rotate 2d vector, given sin and cos
    135 double d2(double,double); ///< distance in 2D
     135void rotate2D(double, double &, double &); ///< rotate 2d vector, given angle
     136void rotate2D(double, double, double &, double &); ///< rotate 2d vector, given sin and cos
     137double d2(double, double); ///< distance in 2D
    136138
    137139#endif
Note: See TracChangeset for help on using the changeset viewer.