IcePlane.h
Go to the documentation of this file.
00001 
00002 
00008 
00009 
00011 // Include Guard
00012 #ifndef __ICEPLANE_H__
00013 #define __ICEPLANE_H__
00014 
00015         #define PLANE_EPSILON           (1.0e-7f)
00016 
00017         class ICEMATHS_API Plane
00018         {
00019                 public:
00021                 inline_                 Plane()                                                                                                                 {                                                                                               }
00023                 inline_                 Plane(float nx, float ny, float nz, float d)                                    { Set(nx, ny, nz, d);                                                   }
00025                 inline_                 Plane(const Point& p, const Point& n)                                                   { Set(p, n);                                                                    }
00027                 inline_                 Plane(const Point& p0, const Point& p1, const Point& p2)                { Set(p0, p1, p2);                                                              }
00029                 inline_                 Plane(const Point& _n, float _d)                                                                { n = _n; d = _d;                                                               }
00031                 inline_                 Plane(const Plane& plane) : n(plane.n), d(plane.d)                              {                                                                                               }
00033                 inline_                 ~Plane()                                                                                                                {                                                                                               }
00034 
00035                 inline_ Plane&  Zero()                                                                                                                  { n.Zero(); d = 0.0f;                   return *this;   }
00036                 inline_ Plane&  Set(float nx, float ny, float nz, float _d)                                             { n.Set(nx, ny, nz); d = _d;    return *this;   }
00037                 inline_ Plane&  Set(const Point& p, const Point& _n)                                                    { n = _n; d = - p | _n;                 return *this;   }
00038                                 Plane&  Set(const Point& p0, const Point& p1, const Point& p2);
00039 
00040                 inline_ float   Distance(const Point& p)                        const                                           { return (p | n) + d;                                                   }
00041                 inline_ bool    Belongs(const Point& p)                         const                                           { return fabsf(Distance(p)) < PLANE_EPSILON;    }
00042 
00043                 inline_ void    Normalize()
00044                                                 {
00045                                                         float Denom = 1.0f / n.Magnitude();
00046                                                         n.x     *= Denom;
00047                                                         n.y     *= Denom;
00048                                                         n.z     *= Denom;
00049                                                         d       *= Denom;
00050                                                 }
00051                 public:
00052                 // Members
00053                                 Point   n;              
00054                                 float   d;              
00055 
00056                 // Cast operators
00057                 inline_                 operator Point()                                        const                                           { return n;                                                                             }
00058                 inline_                 operator HPoint()                                       const                                           { return HPoint(n, d);                                                  }
00059 
00060                 // Arithmetic operators
00061                 inline_ Plane   operator*(const Matrix4x4& m)           const
00062                                                 {
00063                                                         // Old code from Irion. Kept for reference.
00064                                                         Plane Ret(*this);
00065                                                         return Ret *= m;
00066                                                 }
00067 
00068                 inline_ Plane&  operator*=(const Matrix4x4& m)
00069                                                 {
00070                                                         // Old code from Irion. Kept for reference.
00071                                                         Point n2 = HPoint(n, 0.0f) * m;
00072                                                         d = -((Point) (HPoint( -d*n, 1.0f ) * m) | n2);
00073                                                         n = n2;
00074                                                         return *this;
00075                                                 }
00076         };
00077 
00079 
00086 
00087         inline_ void TransformPlane(Plane& transformed, const Plane& plane, const Matrix4x4& transform)
00088         {
00089                 // Rotate the normal using the rotation part of the 4x4 matrix
00090                 transformed.n = plane.n * Matrix3x3(transform);
00091 
00092                 // Compute new d
00093                 transformed.d = plane.d - (Point(transform.GetTrans())|transformed.n);
00094         }
00095 
00097 
00103 
00104         inline_ void TransformPlane(Plane& plane, const Matrix4x4& transform)
00105         {
00106                 // Rotate the normal using the rotation part of the 4x4 matrix
00107                 plane.n *= Matrix3x3(transform);
00108 
00109                 // Compute new d
00110                 plane.d -= Point(transform.GetTrans())|plane.n;
00111         }
00112 
00113 #endif // __ICEPLANE_H__


openhrp3
Author(s): AIST, General Robotix Inc., Nakamura Lab of Dept. of Mechano Informatics at University of Tokyo
autogenerated on Thu Apr 11 2019 03:30:16