Changeset 255
 Timestamp:
 11/18/14 17:03:27 (8 years ago)
 Location:
 cpp/frams/util
 Files:

 2 edited
Legend:
 Unmodified
 Added
 Removed

cpp/frams/util/3d.cpp
r197 r255 7 7 #include "3d.h" 8 8 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.xp2.x,p1.yp2.y,p1.zp2.z);}11 12 Pt3D Pt3D_0(0, 0,0);13 14 bool Pt3D::report_errors =true;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; 15 15 16 16 double Pt3D::operator()() const 17 17 { 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); 21 21 } 22 22 23 23 bool Pt3D::normalize() 24 24 { 25 double len=length();26 if (fabs(len)<1e50) {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) < 1e50) { if (report_errors) FramMessage("Pt3D", "normalize()", "vector too small", 1); x = 1; y = 0; z = 0; return false; } 27 operator/=(len); 28 return true; 29 29 } 30 30 31 31 double Pt3D::distanceTo(const Pt3D& p) const 32 32 { 33 return sqrt((xp.x)*(xp.x)+(yp.y)*(yp.y)+(zp.z)*(zp.z));33 return sqrt((x  p.x)*(x  p.x) + (y  p.y)*(y  p.y) + (z  p.z)*(z  p.z)); 34 34 } 35 35 36 36 double Pt3D::manhattanDistanceTo(const Pt3D& p) const 37 37 { 38 return fabs(xp.x)+fabs(yp.y)+fabs(zp.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 41 Orient Orient_1(Pt3D(1, 0, 0), Pt3D(0, 1, 0), Pt3D(0, 0, 1)); 42 42 43 43 // prosty obrot 44 void rotate2D(double k,double &x,double &y) 45 {double s=sin(k),c=cos(k); 46 double t=c*xs*y; y=s*x+c*y; x=t;} 47 48 void rotate2D(double s,double c,double &x,double &y) 49 {double t=c*xs*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 44 void 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 50 void 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 55 int 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 62 void Pt3D::getAngles(const Pt3D& X, const Pt3D& dir) 63 { 64 Pt3D t1(X), t2(dir); 65 if (getAngle(t1.x, t1.y, z)) // nonvertical 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 78 82 } 79 83 80 84 void Pt3D::getMin(const Pt3D& p) 81 85 { 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; 85 89 } 86 90 void Pt3D::getMax(const Pt3D& p) 87 91 { 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.za.z*b.y; 96 y=a.z*b.xa.x*b.z; 97 z=a.x*b.ya.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 97 void 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 104 void 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 xonly variant 111 } 112 113 void 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 136 double 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); 119 141 } 120 142 121 143 Orient::Orient(const Matrix44& m) 122 144 { 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]; 126 148 } 127 149 128 150 void Orient::operator=(const Pt3D &rot) 129 151 { 130 *this=Orient_1;131 rotate(rot);152 *this = Orient_1; 153 rotate(rot); 132 154 } 133 155 134 156 void Orient::rotate(const Pt3D &v) 135 157 { 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) const161 { 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) const168 { 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) const175 { 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) const182 { 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 182 void 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 189 void 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 196 void 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 203 void 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); 186 208 } 187 209 188 210 void Orient::getAngles(Pt3D &angles) const 189 211 { 190 angles.getAngles(x,z);212 angles.getAngles(x, z); 191 213 } 192 214 193 215 bool Orient::normalize() 194 216 { 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; 202 224 } 203 225 204 226 Matrix44::Matrix44(const Orient &rot) 205 227 { 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; 210 232 } 211 233 
cpp/frams/util/3d.h
r225 r255 15 15 \file 3d.h 3d.cpp 16 16 17 basic 3d classes and operators 17 basic 3D classes and operators 18 18 *********************************/ 19 19 20 /// point in 3d space 21 20 /// point in 3D space 22 21 class Pt3D 23 22 { 24 public: double x,y,z; 25 static bool report_errors; 23 public: 24 double x, y, z; 25 static bool report_errors; 26 26 27 Pt3D(double _x,double _y,double _z):x(_x),y(_y),z(_z) {} ///< constructor initializing all coords28 Pt3D(double xyz):x(xyz),y(xyz),z(xyz) {} ///< all coords equal29 Pt3D() {} ///< coords will be not initialized!30 Pt3D(const Pt3D &p):x(p.x),y(p.y),z(p.z) {} ///< copy from another point31 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.zz*p.y, z*p.xx*p.z, x*p.yy*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(xp.x,yp.y,zp.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 product60 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(xp.x,yp.y,zp.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(); 62 62 }; 63 Pt3D operator+(const Pt3D &p1,const Pt3D &p2); 64 Pt3D operator(const Pt3D &p1,const Pt3D &p2); 63 64 Pt3D operator+(const Pt3D &p1, const Pt3D &p2); 65 Pt3D operator(const Pt3D &p1, const Pt3D &p2); 65 66 66 67 class Pt3D_DontReportErrors 67 68 { 68 bool state;69 bool state; 69 70 public: 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; } 72 73 }; 73 74 74 /// orientation in 3d space = rotation matrix 75 75 /// orientation in 3D space = rotation matrix 76 76 class Matrix44; 77 77 78 78 class Orient 79 79 { 80 public: Pt3D x,y,z; ///< 3 vectors (= 3x3 matrix) 80 public: 81 Pt3D x, y, z; ///< 3 vectors (= 3x3 matrix) 81 82 82 83 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;} 86 87 Orient(const Matrix44& m); 87 88 void operator=(const Pt3D &rot); 88 89 void rotate(const Pt3D &); ///< rotate matrix around 3 axes 89 90 90 void transform(Pt3D &target, const Pt3D &src) const; ///< transform a vector91 void revTransform(Pt3D &target, const Pt3D &src) const; ///< reverse transform92 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; } 94 95 95 void transform(Orient& target, const Orient& src) const; ///< transform other orient96 void revTransform(Orient& target, const Orient& src) const; ///< reverse transform other orient97 Orient transform(const Orient& src) const { Orient o; transform(o,src); return o;} ///< transform other orient98 Orient revTransform(const Orient& src) const { Orient o; revTransform(o,src); return o;} ///< reverse transform other orient96 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 99 100 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; } 102 103 103 104 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. 106 108 107 109 bool normalize(); … … 111 113 { 112 114 public: 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); 118 120 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]; } 123 125 124 void operator+=(const Pt3D &); ///< translate matrix125 void operator*=(const Pt3D &); ///< scale matrix126 void operator*=(double sc); ///< scale matrix126 void operator+=(const Pt3D &); ///< translate matrix 127 void operator*=(const Pt3D &); ///< scale matrix 128 void operator*=(double sc); ///< scale matrix 127 129 }; 128 130 … … 131 133 extern Matrix44 Matrix44_1; ///< standard unit matrix: 1000 0100 0010 0001 132 134 133 void rotate2D(double, double &,double &); ///< rotate 2d vector, given angle134 void rotate2D(double, double,double &,double &); ///< rotate 2d vector, given sin and cos135 double d2(double, double); ///< distance in 2D135 void rotate2D(double, double &, double &); ///< rotate 2d vector, given angle 136 void rotate2D(double, double, double &, double &); ///< rotate 2d vector, given sin and cos 137 double d2(double, double); ///< distance in 2D 136 138 137 139 #endif
Note: See TracChangeset
for help on using the changeset viewer.