Changeset 305 for cpp/frams/util

Timestamp:

01/24/15 03:36:31 (10 years ago)

Author:

Maciej Komosinski

Message:

Increased precision / less optimizations in some rotations and computation of angles

Location:

cpp/frams/util

Files:

• 3d.cpp (modified) (6 diffs)
• 3d.h (modified) (1 diff)

cpp/frams/util/3d.cpp

@@ -31 +31 @@
 double Pt3D::distanceTo(const Pt3D& p) const
 {
-        return sqrt((x - p.x)*(x - p.x) + (y - p.y)*(y - p.y) + (z - p.z)*(z - p.z));
+        double dx = x - p.x;
+        double dy = y - p.y;
+        double dz = z - p.z;
+        return sqrt(dx*dx + dy*dy + dz*dz);
 }
 
@@ -45 +48 @@
 {
         double s = sin(k), c = cos(k);
-        double t = c*x - s*y; y = s*x + c*y; x = t;
+        double t = c*x - s*y;
+        y = s*x + c*y;
+        x = t;
 }
 
 void rotate2D(double s, double c, double &x, double &y)
 {
-        double t = c*x - s*y; y = s*x + c*y; x = t;
-}
-
-int Pt3D::getAngle(double dx, double dy, double &wyn)
-{
-        if ((fabs(dx) + fabs(dy)) < 0.0001) return 0;
-        wyn = atan2(dy, dx);
-        return 1;
+        double t = c*x - s*y;
+        y = s*x + c*y;
+        x = t;
+}
+
+double Pt3D::getAngle(double dx, double dy)
+{
+        if (dx == 0 && dy == 0)
+        {
+                if (report_errors) FMprintf("Pt3D", "getAngle()", FMLV_WARN, "atan2(%g,%g)", dy, dx);
+                return 0; // incorrect result, but there is no correct one
+        }
+        return atan2(dy, dx);
 }
 
 void Pt3D::getAngles(const Pt3D& X, const Pt3D& dir)
 {
-        Pt3D    t1(X), t2(dir);
-        if (getAngle(t1.x, t1.y, z)) // non-vertical
+        Pt3D t1(X), t2(dir);
+        if (fabs(t1.x) > 1e-50 || fabs(t1.y) > 1e-50) // non-vertical
         {
                 rotate2D(-z, t1.x, t1.y);
                 rotate2D(-z, t2.x, t2.y);
-                getAngle(t1.x, t1.z, y);
+                y = getAngle(t1.x, t1.z);
         }
         else // vertical
@@ -78 +88 @@
         }
         rotate2D(-y, t2.x, t2.z);
-        if (!getAngle(t2.z, -t2.y, x))
-                x = 0; // incorrect result, but there is no correct one
+        x = getAngle(t2.z, -t2.y);
 }
 
@@ -157 +166 @@
 {
         double s, c;
-        if (fabs(v.x) > 0.0001)
+        if (v.x != 0)
         {
                 s = sin(v.x); c = cos(v.x);
@@ -164 +173 @@
                 rotate2D(s, c, z.y, z.z);
         }
-        if (fabs(v.y) > 0.0001)
+        if (v.y != 0)
         {
                 s = sin(v.y); c = cos(v.y);
@@ -171 +180 @@
                 rotate2D(s, c, z.x, z.z);
         }
-        if (fabs(v.z) > 0.0001)
+        if (v.z != 0)
         {
                 s = sin(v.z); c = cos(v.z);

cpp/frams/util/3d.h

@@ -52 +52 @@
         double manhattanDistanceTo(const Pt3D& p) const;
         /** calculate angle between (0,0)-(dx,dy), @return 1=ok, 0=can't calculate */
-        static int getAngle(double dx, double dy, double &angle);
+        static double getAngle(double dx, double dy);
         /** calculate 3 rotation angles translating (1,0,0) into 'X' and (0,0,1) into 'dir' */
         void getAngles(const Pt3D& X, const Pt3D& dir);