Changeset 305 for cpp/frams/util
- Timestamp:
- 01/24/15 03:36:31 (10 years ago)
- Location:
- cpp/frams/util
- Files:
-
- 2 edited
Legend:
- Unmodified
- Added
- Removed
-
cpp/frams/util/3d.cpp
r303 r305 31 31 double Pt3D::distanceTo(const Pt3D& p) const 32 32 { 33 return sqrt((x - p.x)*(x - p.x) + (y - p.y)*(y - p.y) + (z - p.z)*(z - p.z)); 33 double dx = x - p.x; 34 double dy = y - p.y; 35 double dz = z - p.z; 36 return sqrt(dx*dx + dy*dy + dz*dz); 34 37 } 35 38 … … 45 48 { 46 49 double s = sin(k), c = cos(k); 47 double t = c*x - s*y; y = s*x + c*y; x = t; 50 double t = c*x - s*y; 51 y = s*x + c*y; 52 x = t; 48 53 } 49 54 50 55 void rotate2D(double s, double c, double &x, double &y) 51 56 { 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; 57 double t = c*x - s*y; 58 y = s*x + c*y; 59 x = t; 60 } 61 62 double Pt3D::getAngle(double dx, double dy) 63 { 64 if (dx == 0 && dy == 0) 65 { 66 if (report_errors) FMprintf("Pt3D", "getAngle()", FMLV_WARN, "atan2(%g,%g)", dy, dx); 67 return 0; // incorrect result, but there is no correct one 68 } 69 return atan2(dy, dx); 60 70 } 61 71 62 72 void Pt3D::getAngles(const Pt3D& X, const Pt3D& dir) 63 73 { 64 Pt3D 65 if ( getAngle(t1.x, t1.y, z)) // non-vertical74 Pt3D t1(X), t2(dir); 75 if (fabs(t1.x) > 1e-50 || fabs(t1.y) > 1e-50) // non-vertical 66 76 { 67 77 rotate2D(-z, t1.x, t1.y); 68 78 rotate2D(-z, t2.x, t2.y); 69 getAngle(t1.x, t1.z, y);79 y = getAngle(t1.x, t1.z); 70 80 } 71 81 else // vertical … … 78 88 } 79 89 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 90 x = getAngle(t2.z, -t2.y); 82 91 } 83 92 … … 157 166 { 158 167 double s, c; 159 if ( fabs(v.x) > 0.0001)168 if (v.x != 0) 160 169 { 161 170 s = sin(v.x); c = cos(v.x); … … 164 173 rotate2D(s, c, z.y, z.z); 165 174 } 166 if ( fabs(v.y) > 0.0001)175 if (v.y != 0) 167 176 { 168 177 s = sin(v.y); c = cos(v.y); … … 171 180 rotate2D(s, c, z.x, z.z); 172 181 } 173 if ( fabs(v.z) > 0.0001)182 if (v.z != 0) 174 183 { 175 184 s = sin(v.z); c = cos(v.z); -
cpp/frams/util/3d.h
r286 r305 52 52 double manhattanDistanceTo(const Pt3D& p) const; 53 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);54 static double getAngle(double dx, double dy); 55 55 /** calculate 3 rotation angles translating (1,0,0) into 'X' and (0,0,1) into 'dir' */ 56 56 void getAngles(const Pt3D& X, const Pt3D& dir);
Note: See TracChangeset
for help on using the changeset viewer.