IcePoint.h
Go to the documentation of this file.
00001 
00002 
00008 
00009 
00011 // Include Guard
00012 #ifndef __ICEPOINT_H__
00013 #define __ICEPOINT_H__
00014 
00015         // Forward declarations
00016         class HPoint;
00017         class Plane;
00018         class Matrix3x3;
00019         class Matrix4x4;
00020 
00021         #define CROSS2D(a, b)   (a.x*b.y - b.x*a.y)
00022 
00023         const float EPSILON2 = 1.0e-20f;
00024 
00025         class ICEMATHS_API Point
00026         {
00027                 public:
00028 
00030                 inline_                                 Point()                                                                                                         {}
00032 //              inline_                                 Point(float val) : x(val), y(val), z(val)                                       {}
00033 // Removed since it introduced the nasty "Point T = *Matrix4x4.GetTrans();" bug.......
00035                 inline_                                 Point(float _x, float _y, float _z) : x(_x), y(_y), z(_z)       {}
00037                 inline_                                 Point(const float f[3]) : x(f[_X]), y(f[_Y]), z(f[_Z])          {}
00039                 inline_                                 Point(const Point& p) : x(p.x), y(p.y), z(p.z)                          {}
00041                 inline_                                 ~Point()                                                                                                        {}
00042 
00044                 inline_ Point&                  Zero()                                                                  { x =                   y =                     z = 0.0f;                       return *this;   }
00045 
00047                 inline_ Point&                  SetPlusInfinity()                                               { x =                   y =                     z = MAX_FLOAT;          return *this;   }
00049                 inline_ Point&                  SetMinusInfinity()                                              { x =                   y =                     z = MIN_FLOAT;          return *this;   }
00050 
00052                                 Point&                  PositiveUnitRandomVector();
00054                                 Point&                  UnitRandomVector();
00055 
00057                 inline_ Point&                  Set(float _x, float _y, float _z)               { x  = _x;              y  = _y;        z  = _z;                        return *this;   }
00059                 inline_ Point&                  Set(const float f[3])                                   { x  = f[_X];   y  = f[_Y];     z  = f[_Z];                     return *this;   }
00061                 inline_ Point&                  Set(const Point& src)                                   { x  = src.x;   y  = src.y;     z  = src.z;                     return *this;   }
00062 
00064                 inline_ Point&                  Add(const Point& p)                                             { x += p.x;             y += p.y;       z += p.z;                       return *this;   }
00066                 inline_ Point&                  Add(float _x, float _y, float _z)               { x += _x;              y += _y;        z += _z;                        return *this;   }
00068                 inline_ Point&                  Add(const float f[3])                                   { x += f[_X];   y += f[_Y];     z += f[_Z];                     return *this;   }
00070                 inline_ Point&                  Add(const Point& p, const Point& q)             { x = p.x+q.x;  y = p.y+q.y;    z = p.z+q.z;    return *this;   }
00071 
00073                 inline_ Point&                  Sub(const Point& p)                                             { x -= p.x;             y -= p.y;       z -= p.z;                       return *this;   }
00075                 inline_ Point&                  Sub(float _x, float _y, float _z)               { x -= _x;              y -= _y;        z -= _z;                        return *this;   }
00077                 inline_ Point&                  Sub(const float f[3])                                   { x -= f[_X];   y -= f[_Y];     z -= f[_Z];                     return *this;   }
00079                 inline_ Point&                  Sub(const Point& p, const Point& q)             { x = p.x-q.x;  y = p.y-q.y;    z = p.z-q.z;    return *this;   }
00080 
00082                 inline_ Point&                  Neg()                                                                   { x = -x;               y = -y;                 z = -z;                 return *this;   }
00084                 inline_ Point&                  Neg(const Point& a)                                             { x = -a.x;             y = -a.y;               z = -a.z;               return *this;   }
00085 
00087                 inline_ Point&                  Mult(float s)                                                   { x *= s;               y *= s;         z *= s;                         return *this;   }
00088 
00090                 inline_ Point&                  Mult(const Point& a, float scalar)
00091                                                                 {
00092                                                                         x = a.x * scalar;
00093                                                                         y = a.y * scalar;
00094                                                                         z = a.z * scalar;
00095                                                                         return *this;
00096                                                                 }
00097 
00099                 inline_ Point&                  Mac(const Point& a, const Point& b, float scalar)
00100                                                                 {
00101                                                                         x = a.x + b.x * scalar;
00102                                                                         y = a.y + b.y * scalar;
00103                                                                         z = a.z + b.z * scalar;
00104                                                                         return *this;
00105                                                                 }
00106 
00108                 inline_ Point&                  Mac(const Point& a, float scalar)
00109                                                                 {
00110                                                                         x += a.x * scalar;
00111                                                                         y += a.y * scalar;
00112                                                                         z += a.z * scalar;
00113                                                                         return *this;
00114                                                                 }
00115 
00117                 inline_ Point&                  Msc(const Point& a, const Point& b, float scalar)
00118                                                                 {
00119                                                                         x = a.x - b.x * scalar;
00120                                                                         y = a.y - b.y * scalar;
00121                                                                         z = a.z - b.z * scalar;
00122                                                                         return *this;
00123                                                                 }
00124 
00126                 inline_ Point&                  Msc(const Point& a, float scalar)
00127                                                                 {
00128                                                                         x -= a.x * scalar;
00129                                                                         y -= a.y * scalar;
00130                                                                         z -= a.z * scalar;
00131                                                                         return *this;
00132                                                                 }
00133 
00135                 inline_ Point&                  Mac2(const Point& a, const Point& b, float scalarb, const Point& c, float scalarc)
00136                                                                 {
00137                                                                         x = a.x + b.x * scalarb + c.x * scalarc;
00138                                                                         y = a.y + b.y * scalarb + c.y * scalarc;
00139                                                                         z = a.z + b.z * scalarb + c.z * scalarc;
00140                                                                         return *this;
00141                                                                 }
00142 
00144                 inline_ Point&                  Msc2(const Point& a, const Point& b, float scalarb, const Point& c, float scalarc)
00145                                                                 {
00146                                                                         x = a.x - b.x * scalarb - c.x * scalarc;
00147                                                                         y = a.y - b.y * scalarb - c.y * scalarc;
00148                                                                         z = a.z - b.z * scalarb - c.z * scalarc;
00149                                                                         return *this;
00150                                                                 }
00151 
00153                 inline_ Point&                  Mult(const Matrix3x3& mat, const Point& a);
00154 
00156                 inline_ Point&                  Mult2(const Matrix3x3& mat1, const Point& a1, const Matrix3x3& mat2, const Point& a2);
00157 
00159                 inline_ Point&                  Mac(const Matrix3x3& mat, const Point& a);
00160 
00162                 inline_ Point&                  TransMult(const Matrix3x3& mat, const Point& a);
00163 
00165                 inline_ Point&                  Lerp(const Point& a, const Point& b, float t)
00166                                                                 {
00167                                                                         x = a.x + t * (b.x - a.x);
00168                                                                         y = a.y + t * (b.y - a.y);
00169                                                                         z = a.z + t * (b.z - a.z);
00170                                                                         return *this;
00171                                                                 }
00172 
00178                 inline_ Point&                  Herp(const Point& p0, const Point& p1, const Point& p2, const Point& p3, float t)
00179                                                                 {
00180                                                                         float t2 = t * t;
00181                                                                         float t3 = t2 * t;
00182                                                                         float kp0 = (2.0f * t2 - t3 - t) * 0.5f;
00183                                                                         float kp1 = (3.0f * t3 - 5.0f * t2 + 2.0f) * 0.5f;
00184                                                                         float kp2 = (4.0f * t2 - 3.0f * t3 + t) * 0.5f;
00185                                                                         float kp3 = (t3 - t2) * 0.5f;
00186                                                                         x = p0.x * kp0 + p1.x * kp1 + p2.x * kp2 + p3.x * kp3;
00187                                                                         y = p0.y * kp0 + p1.y * kp1 + p2.y * kp2 + p3.y * kp3;
00188                                                                         z = p0.z * kp0 + p1.z * kp1 + p2.z * kp2 + p3.z * kp3;
00189                                                                         return *this;
00190                                                                 }
00191 
00193                 inline_ Point&                  Transform(const Point& r, const Matrix3x3& rotpos, const Point& linpos);
00194 
00196                 inline_ Point&                  InvTransform(const Point& r, const Matrix3x3& rotpos, const Point& linpos);
00197 
00199                 inline_ float                   Min()                           const           { return MIN(x, MIN(y, z));                                                                                             }
00201                 inline_ float                   Max()                           const           { return MAX(x, MAX(y, z));                                                                                             }
00203                 inline_ Point&                  Min(const Point& p)                             { x = MIN(x, p.x); y = MIN(y, p.y); z = MIN(z, p.z);    return *this;   }
00205                 inline_ Point&                  Max(const Point& p)                             { x = MAX(x, p.x); y = MAX(y, p.y); z = MAX(z, p.z);    return *this;   }
00206 
00208                 inline_ Point&                  Clamp(float min, float max)
00209                                                                 {
00210                                                                         if(x<min)       x=min;  if(x>max)       x=max;
00211                                                                         if(y<min)       y=min;  if(y>max)       y=max;
00212                                                                         if(z<min)       z=min;  if(z>max)       z=max;
00213                                                                         return *this;
00214                                                                 }
00215 
00217                 inline_ float                   SquareMagnitude()       const           { return x*x + y*y + z*z;                                                                                               }
00219                 inline_ float                   Magnitude()                     const           { return sqrtf(x*x + y*y + z*z);                                                                                }
00221                 inline_ float                   Volume()                        const           { return x * y * z;                                                                                                             }
00222 
00224                 inline_ bool                    ApproxZero()            const           { return SquareMagnitude() < EPSILON2;                                                                  }
00225 
00227                 inline_ BOOL                    IsZero()                        const
00228                                                                 {
00229                                                                         if(IR(x) || IR(y) || IR(z))     return FALSE;
00230                                                                         return TRUE;
00231                                                                 }
00232 
00234                 inline_ BOOL                    IsValid()                       const
00235                                                                 {
00236                                                                         if(!IsValidFloat(x))    return FALSE;
00237                                                                         if(!IsValidFloat(y))    return FALSE;
00238                                                                         if(!IsValidFloat(z))    return FALSE;
00239                                                                         return TRUE;
00240                                                                 }
00241 
00243                                 void                    Tweak(udword coord_mask, udword tweak_mask)
00244                                                                 {
00245                                                                         if(coord_mask&1)        { udword Dummy = IR(x); Dummy^=tweak_mask;      x = FR(Dummy); }
00246                                                                         if(coord_mask&2)        { udword Dummy = IR(y); Dummy^=tweak_mask;      y = FR(Dummy); }
00247                                                                         if(coord_mask&4)        { udword Dummy = IR(z); Dummy^=tweak_mask;      z = FR(Dummy); }
00248                                                                 }
00249 
00250                 #define TWEAKMASK               0x3fffff
00251                 #define TWEAKNOTMASK    ~TWEAKMASK
00252 
00253                 inline_ void                    TweakBigger()
00254                                                                 {
00255                                                                         udword  Dummy = (IR(x)&TWEAKNOTMASK);   if(!IS_NEGATIVE_FLOAT(x))       Dummy+=TWEAKMASK+1;     x = FR(Dummy);
00256                                                                                         Dummy = (IR(y)&TWEAKNOTMASK);   if(!IS_NEGATIVE_FLOAT(y))       Dummy+=TWEAKMASK+1;     y = FR(Dummy);
00257                                                                                         Dummy = (IR(z)&TWEAKNOTMASK);   if(!IS_NEGATIVE_FLOAT(z))       Dummy+=TWEAKMASK+1;     z = FR(Dummy);
00258                                                                 }
00259 
00261                 inline_ void                    TweakSmaller()
00262                                                                 {
00263                                                                         udword  Dummy = (IR(x)&TWEAKNOTMASK);   if(IS_NEGATIVE_FLOAT(x))        Dummy+=TWEAKMASK+1;     x = FR(Dummy);
00264                                                                                         Dummy = (IR(y)&TWEAKNOTMASK);   if(IS_NEGATIVE_FLOAT(y))        Dummy+=TWEAKMASK+1;     y = FR(Dummy);
00265                                                                                         Dummy = (IR(z)&TWEAKNOTMASK);   if(IS_NEGATIVE_FLOAT(z))        Dummy+=TWEAKMASK+1;     z = FR(Dummy);
00266                                                                 }
00267 
00269                 inline_ Point&                  Normalize()
00270                                                                 {
00271                                                                         float M = x*x + y*y + z*z;
00272                                                                         if(M)
00273                                                                         {
00274                                                                                 M = 1.0f / sqrtf(M);
00275                                                                                 x *= M;
00276                                                                                 y *= M;
00277                                                                                 z *= M;
00278                                                                         }
00279                                                                         return *this;
00280                                                                 }
00281 
00283                 inline_ Point&                  SetLength(float length)
00284                                                                 {
00285                                                                         float NewLength = length / Magnitude();
00286                                                                         x *= NewLength;
00287                                                                         y *= NewLength;
00288                                                                         z *= NewLength;
00289                                                                         return *this;
00290                                                                 }
00291 
00293                 inline_ Point&                  ClampLength(float limit_length)
00294                                                                 {
00295                                                                         if(limit_length>=0.0f)  // Magnitude must be positive
00296                                                                         {
00297                                                                                 float CurrentSquareLength = SquareMagnitude();
00298 
00299                                                                                 if(CurrentSquareLength > limit_length * limit_length)
00300                                                                                 {
00301                                                                                         float Coeff = limit_length / sqrtf(CurrentSquareLength);
00302                                                                                         x *= Coeff;
00303                                                                                         y *= Coeff;
00304                                                                                         z *= Coeff;
00305                                                                                 }
00306                                                                         }
00307                                                                         return *this;
00308                                                                 }
00309 
00311                 inline_ float                   Distance(const Point& b)                        const
00312                                                                 {
00313                                                                         return sqrtf((x - b.x)*(x - b.x) + (y - b.y)*(y - b.y) + (z - b.z)*(z - b.z));
00314                                                                 }
00315 
00317                 inline_ float                   SquareDistance(const Point& b)          const
00318                                                                 {
00319                                                                         return ((x - b.x)*(x - b.x) + (y - b.y)*(y - b.y) + (z - b.z)*(z - b.z));
00320                                                                 }
00321 
00323                 inline_ float                   Dot(const Point& p)                                     const           {       return p.x * x + p.y * y + p.z * z;                             }
00324 
00326                 inline_ Point&                  Cross(const Point& a, const Point& b)
00327                                                                 {
00328                                                                         x = a.y * b.z - a.z * b.y;
00329                                                                         y = a.z * b.x - a.x * b.z;
00330                                                                         z = a.x * b.y - a.y * b.x;
00331                                                                         return *this;
00332                                                                 }
00333 
00335                 inline_ udword                  VectorCode()                                            const
00336                                                                 {
00337                                                                         return (IR(x)>>31) | ((IR(y)&SIGN_BITMASK)>>30) | ((IR(z)&SIGN_BITMASK)>>29);
00338                                                                 }
00339 
00341                 inline_ PointComponent  LargestAxis()                                           const
00342                                                                 {
00343                                                                         //const float* Vals = &x;
00344                                                                         PointComponent m = _X;
00345                                                                         //if(Vals[_Y] > Vals[m]) m = _Y;
00346                                                                         //if(Vals[_Z] > Vals[m]) m = _Z;
00347                                                                         y > x ? (z > y ? m = _Z : m = _Y) : (z > x ? m = _Z : m = _X);  
00348                                                                         return m;
00349                                                                 }
00350 
00352                 inline_ PointComponent  ClosestAxis()                                           const
00353                                                                 {
00354                                                                         const float* Vals = &x;
00355                                                                         PointComponent m = _X;
00356                                                                         if(AIR(Vals[_Y]) > AIR(Vals[m])) m = _Y;
00357                                                                         if(AIR(Vals[_Z]) > AIR(Vals[m])) m = _Z;
00358                                                                         return m;
00359                                                                 }
00360 
00362                 inline_ PointComponent  SmallestAxis()                                          const
00363                                                                 {
00364                                                                         //const float* Vals = &x;
00365                                                                         PointComponent m = _X;
00366                                                                         //if(Vals[_Y] < Vals[m]) m = _Y;
00367                                                                         //if(Vals[_Z] < Vals[m]) m = _Z;
00368                                                                         y < x ? (z < y ? m = _Z : m = _Y) : (z < x ? m = _Z : m = _X);  
00369                                                                         return m;
00370                                                                 }
00371 
00373                                 Point&                  Refract(const Point& eye, const Point& n, float refractindex, Point& refracted);
00374 
00376                                 Point&                  ProjectToPlane(const Plane& p);
00377 
00379                                 void                    ProjectToScreen(float halfrenderwidth, float halfrenderheight, const Matrix4x4& mat, HPoint& projected) const;
00380 
00382                                 Point&                  Unfold(Plane& p, Point& a, Point& b);
00383 
00385                 inline_ udword                  GetHashValue()                                          const
00386                                                                 {
00387                                                                         const udword* h = (const udword*)(this);
00388                                                                         udword f = (h[0]+h[1]*11-(h[2]*17)) & 0x7fffffff;       // avoid problems with +-0
00389                                                                         return (f>>22)^(f>>12)^(f);
00390                                                                 }
00391 
00393                                 void                    SetNotUsed();
00395                                 BOOL                    IsNotUsed()                                                     const;
00396 
00397                 // Arithmetic operators
00398 
00400                 inline_ Point                   operator-()                                                     const           { return Point(-x, -y, -z);                                                     }
00401 
00403                 inline_ Point                   operator+(const Point& p)                       const           { return Point(x + p.x, y + p.y, z + p.z);                      }
00405                 inline_ Point                   operator-(const Point& p)                       const           { return Point(x - p.x, y - p.y, z - p.z);                      }
00406 
00408                 inline_ Point                   operator*(const Point& p)                       const           { return Point(x * p.x, y * p.y, z * p.z);                      }
00410                 inline_ Point                   operator*(float s)                                      const           { return Point(x * s,   y * s,   z * s );                       }
00412                 inline_ friend  Point   operator*(float s, const Point& p)                              { return Point(s * p.x, s * p.y, s * p.z);                      }
00413 
00415                 inline_ Point                   operator/(const Point& p)                       const           { return Point(x / p.x, y / p.y, z / p.z);                      }
00417                 inline_ Point                   operator/(float s)                                      const           { s = 1.0f / s; return Point(x * s, y * s, z * s);      }
00419                 inline_ friend  Point   operator/(float s, const Point& p)                              { return Point(s / p.x, s / p.y, s / p.z);                      }
00420 
00422                 inline_ float                   operator|(const Point& p)                       const           { return x*p.x + y*p.y + z*p.z;                                         }
00424                 inline_ Point                   operator^(const Point& p)                       const
00425                                                                 {
00426                                                                         return Point(
00427                                                                         y * p.z - z * p.y,
00428                                                                         z * p.x - x * p.z,
00429                                                                         x * p.y - y * p.x );
00430                                                                 }
00431 
00433                 inline_ Point&                  operator+=(const Point& p)                                              { x += p.x; y += p.y; z += p.z; return *this;           }
00435                 inline_ Point&                  operator+=(float s)                                                             { x += s;   y += s;   z += s;   return *this;           }
00436 
00438                 inline_ Point&                  operator-=(const Point& p)                                              { x -= p.x; y -= p.y; z -= p.z; return *this;           }
00440                 inline_ Point&                  operator-=(float s)                                                             { x -= s;   y -= s;   z -= s;   return *this;           }
00441 
00443                 inline_ Point&                  operator*=(const Point& p)                                              { x *= p.x; y *= p.y; z *= p.z; return *this;           }
00445                 inline_ Point&                  operator*=(float s)                                                             { x *= s; y *= s; z *= s;               return *this;           }
00446 
00448                 inline_ Point&                  operator/=(const Point& p)                                              { x /= p.x; y /= p.y; z /= p.z; return *this;           }
00450                 inline_ Point&                  operator/=(float s)                                                             { s = 1.0f/s; x *= s; y *= s; z *= s; return *this; }
00451 
00452                 // Logical operators
00453 
00455                 inline_ bool                    operator==(const Point& p)                      const           { return ( (IR(x)==IR(p.x))&&(IR(y)==IR(p.y))&&(IR(z)==IR(p.z)));       }
00457                 inline_ bool                    operator!=(const Point& p)                      const           { return ( (IR(x)!=IR(p.x))||(IR(y)!=IR(p.y))||(IR(z)!=IR(p.z)));       }
00458 
00459                 // Arithmetic operators
00460 
00462                 inline_ Point                   operator*(const Matrix3x3& mat)         const
00463                                                                 {
00464                                                                         class ShadowMatrix3x3{ public: float m[3][3]; };        // To allow inlining
00465                                                                         const ShadowMatrix3x3* Mat = (const ShadowMatrix3x3*)&mat;
00466 
00467                                                                         return Point(
00468                                                                         x * Mat->m[0][0] + y * Mat->m[1][0] + z * Mat->m[2][0],
00469                                                                         x * Mat->m[0][1] + y * Mat->m[1][1] + z * Mat->m[2][1],
00470                                                                         x * Mat->m[0][2] + y * Mat->m[1][2] + z * Mat->m[2][2] );
00471                                                                 }
00472 
00474                 inline_ Point                   operator*(const Matrix4x4& mat)         const
00475                                                                 {
00476                                                                         class ShadowMatrix4x4{ public: float m[4][4]; };        // To allow inlining
00477                                                                         const ShadowMatrix4x4* Mat = (const ShadowMatrix4x4*)&mat;
00478 
00479                                                                         return Point(
00480                                                                         x * Mat->m[0][0] + y * Mat->m[1][0] + z * Mat->m[2][0] + Mat->m[3][0],
00481                                                                         x * Mat->m[0][1] + y * Mat->m[1][1] + z * Mat->m[2][1] + Mat->m[3][1],
00482                                                                         x * Mat->m[0][2] + y * Mat->m[1][2] + z * Mat->m[2][2] + Mat->m[3][2]);
00483                                                                 }
00484 
00486                 inline_ Point&                  operator*=(const Matrix3x3& mat)
00487                                                                 {
00488                                                                         class ShadowMatrix3x3{ public: float m[3][3]; };        // To allow inlining
00489                                                                         const ShadowMatrix3x3* Mat = (const ShadowMatrix3x3*)&mat;
00490 
00491                                                                         float xp = x * Mat->m[0][0] + y * Mat->m[1][0] + z * Mat->m[2][0];
00492                                                                         float yp = x * Mat->m[0][1] + y * Mat->m[1][1] + z * Mat->m[2][1];
00493                                                                         float zp = x * Mat->m[0][2] + y * Mat->m[1][2] + z * Mat->m[2][2];
00494 
00495                                                                         x = xp; y = yp; z = zp;
00496 
00497                                                                         return *this;
00498                                                                 }
00499 
00501                 inline_ Point&                  operator*=(const Matrix4x4& mat)
00502                                                                 {
00503                                                                         class ShadowMatrix4x4{ public: float m[4][4]; };        // To allow inlining
00504                                                                         const ShadowMatrix4x4* Mat = (const ShadowMatrix4x4*)&mat;
00505 
00506                                                                         float xp = x * Mat->m[0][0] + y * Mat->m[1][0] + z * Mat->m[2][0] + Mat->m[3][0];
00507                                                                         float yp = x * Mat->m[0][1] + y * Mat->m[1][1] + z * Mat->m[2][1] + Mat->m[3][1];
00508                                                                         float zp = x * Mat->m[0][2] + y * Mat->m[1][2] + z * Mat->m[2][2] + Mat->m[3][2];
00509 
00510                                                                         x = xp; y = yp; z = zp;
00511 
00512                                                                         return *this;
00513                                                                 }
00514 
00515                 // Cast operators
00516 
00518                                                                 operator        HPoint()                                const;
00519 
00520                 inline_                                 operator        const   float*() const  { return &x; }
00521                 inline_                                 operator                        float*()                { return &x; }
00522 
00523                 public:
00524                                 float                   x, y, z;
00525         };
00526 
00527         FUNCTION ICEMATHS_API void Normalize1(Point& a);
00528         FUNCTION ICEMATHS_API void Normalize2(Point& a);
00529 
00530 #endif //__ICEPOINT_H__


openhrp3
Author(s): AIST, General Robotix Inc., Nakamura Lab of Dept. of Mechano Informatics at University of Tokyo
autogenerated on Sun Apr 2 2017 03:43:54