Changeset 255 for cpp/frams/util/3d.cpp
 Timestamp:
 11/18/14 17:03:27 (9 years ago)
 File:

 1 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
Note: See TracChangeset
for help on using the changeset viewer.