Changeset 305 for cpp/frams/util


Ignore:
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:
2 edited

Legend:

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

    r303 r305  
    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        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);
    3437}
    3538
     
    4548{
    4649        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;
    4853}
    4954
    5055void rotate2D(double s, double c, double &x, double &y)
    5156{
    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
     62double 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);
    6070}
    6171
    6272void Pt3D::getAngles(const Pt3D& X, const Pt3D& dir)
    6373{
    64         Pt3D    t1(X), t2(dir);
    65         if (getAngle(t1.x, t1.y, z)) // non-vertical
     74        Pt3D t1(X), t2(dir);
     75        if (fabs(t1.x) > 1e-50 || fabs(t1.y) > 1e-50) // non-vertical
    6676        {
    6777                rotate2D(-z, t1.x, t1.y);
    6878                rotate2D(-z, t2.x, t2.y);
    69                 getAngle(t1.x, t1.z, y);
     79                y = getAngle(t1.x, t1.z);
    7080        }
    7181        else // vertical
     
    7888        }
    7989        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);
    8291}
    8392
     
    157166{
    158167        double s, c;
    159         if (fabs(v.x) > 0.0001)
     168        if (v.x != 0)
    160169        {
    161170                s = sin(v.x); c = cos(v.x);
     
    164173                rotate2D(s, c, z.y, z.z);
    165174        }
    166         if (fabs(v.y) > 0.0001)
     175        if (v.y != 0)
    167176        {
    168177                s = sin(v.y); c = cos(v.y);
     
    171180                rotate2D(s, c, z.x, z.z);
    172181        }
    173         if (fabs(v.z) > 0.0001)
     182        if (v.z != 0)
    174183        {
    175184                s = sin(v.z); c = cos(v.z);
  • cpp/frams/util/3d.h

    r286 r305  
    5252        double manhattanDistanceTo(const Pt3D& p) const;
    5353        /** 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);
    5555        /** calculate 3 rotation angles translating (1,0,0) into 'X' and (0,0,1) into 'dir' */
    5656        void getAngles(const Pt3D& X, const Pt3D& dir);
Note: See TracChangeset for help on using the changeset viewer.