Panda3D

collisionBox.I

00001 // Filename: collisionBox.I
00002 // Created by:  amith tudur (31Jul09)
00003 //
00004 ////////////////////////////////////////////////////////////////////
00005 //
00006 // PANDA 3D SOFTWARE
00007 // Copyright (c) Carnegie Mellon University.  All rights reserved.
00008 //
00009 // All use of this software is subject to the terms of the revised BSD
00010 // license.  You should have received a copy of this license along
00011 // with this source code in a file named "LICENSE."
00012 //
00013 ////////////////////////////////////////////////////////////////////
00014 
00015 
00016 ////////////////////////////////////////////////////////////////////
00017 //     Function: CollisionBox::Constructor
00018 //       Access: Public
00019 //  Description: Create the Box by giving a Center and distances of
00020 //               of each of the sides of box from the Center.
00021 ////////////////////////////////////////////////////////////////////
00022 INLINE CollisionBox::
00023 CollisionBox(const LPoint3f &center, float x, float y, float z) :
00024   _center(center), _x(x), _y(y), _z(z)
00025 {
00026   _min = LPoint3f(_center.get_x() - _x, _center.get_y() - _y, _center.get_z() - _z);
00027   _max = LPoint3f(_center.get_x() + _x, _center.get_y() + _y, _center.get_z() + _z); 
00028   _radius = sqrt(_x*_x + _y*_y + _z*_z);
00029   for(int v = 0; v < 8; v++)
00030     _vertex[v] = get_point_aabb(v);
00031   for(int p = 0; p < 6; p++)
00032     _planes[p] = set_plane(p);
00033   setup_box();
00034 }
00035 
00036 ////////////////////////////////////////////////////////////////////
00037 //     Function: CollisionBox::Constructor
00038 //       Access: Public
00039 //  Description: Create the Box by Specifying the Diagonal Points
00040 ////////////////////////////////////////////////////////////////////
00041 INLINE CollisionBox::
00042 CollisionBox(const LPoint3f &min, const LPoint3f &max) :
00043   _min(min), _max(max)
00044 {
00045   _center = (_min + _max) / 2;
00046   _x = _center.get_x() - _min.get_x();
00047   _y = _center.get_y() - _min.get_y();
00048   _z = _center.get_z() - _min.get_z();
00049   _radius = sqrt(_x*_x + _y*_y + _z*_z);
00050   for(int v = 0; v < 8; v++)
00051     _vertex[v] = get_point_aabb(v);
00052   for(int p = 0; p < 6; p++)
00053     _planes[p] = set_plane(p);
00054   setup_box();
00055 }
00056 
00057 ////////////////////////////////////////////////////////////////////
00058 //     Function: CollisionBox::Constructor
00059 //       Access: Public
00060 //  Description: Center as three separate co-ordinate points
00061 ////////////////////////////////////////////////////////////////////
00062 INLINE CollisionBox::
00063 CollisionBox(float cx, float cy, float cz, float x, float y, float z) 
00064 {
00065   CollisionBox(LPoint3f(cx, cy, cz), x, y, z);
00066 }
00067 
00068 ////////////////////////////////////////////////////////////////////
00069 //     Function: CollisionBox::Default constructor
00070 //       Access: Protected
00071 //  Description: Creates an invalid Box.  Only used when reading
00072 //               from a bam file.
00073 ////////////////////////////////////////////////////////////////////
00074 INLINE CollisionBox::
00075 CollisionBox() {
00076 }
00077 
00078 ////////////////////////////////////////////////////////////////////
00079 //     Function: CollisionBox::Copy Constructor
00080 //       Access: Public
00081 //  Description:
00082 ////////////////////////////////////////////////////////////////////
00083 INLINE CollisionBox::
00084 CollisionBox(const CollisionBox &copy) :
00085   CollisionSolid(copy),
00086   _center(copy._center),
00087   _x( copy._x ),
00088   _y( copy._y ),
00089   _z( copy._z ),
00090   _min( copy._min),
00091   _max( copy._max),
00092   _radius( copy._radius )
00093 {
00094   for(int v = 0; v < 8; v++)
00095     _vertex[v] = copy._vertex[v];
00096   for(int p = 0; p < 6; p++)
00097     _planes[p] = copy._planes[p];
00098   setup_box();
00099 }
00100 
00101 ////////////////////////////////////////////////////////////////////
00102 //     Function: CollisionBox::flush_level
00103 //       Access: Public, Static
00104 //  Description: Flushes the PStatCollectors used during traversal.
00105 ////////////////////////////////////////////////////////////////////
00106 INLINE void CollisionBox::
00107 flush_level() {
00108   _volume_pcollector.flush_level();
00109   _test_pcollector.flush_level();
00110 }
00111 
00112 ////////////////////////////////////////////////////////////////////
00113 //     Function: CollisionBox::set_center
00114 //       Access: Published
00115 //  Description:
00116 ////////////////////////////////////////////////////////////////////
00117 INLINE void CollisionBox::
00118 set_center(const LPoint3f &center) {
00119   _center = center;
00120   mark_internal_bounds_stale();
00121   mark_viz_stale();
00122 }
00123 
00124 ////////////////////////////////////////////////////////////////////
00125 //     Function: CollisionBox::set_center
00126 //       Access: Published
00127 //  Description:
00128 ////////////////////////////////////////////////////////////////////
00129 INLINE void CollisionBox::
00130 set_center(float x, float y, float z) {
00131   set_center(LPoint3f(x, y, z));
00132 }
00133 
00134 ////////////////////////////////////////////////////////////////////
00135 //     Function: CollisionBox::get_center
00136 //       Access: Published
00137 //  Description:
00138 ////////////////////////////////////////////////////////////////////
00139 INLINE const LPoint3f &CollisionBox::
00140 get_center() const {
00141   return _center;
00142 }
00143 
00144 ////////////////////////////////////////////////////////////////////
00145 //     Function: CollisionBox::get_radius
00146 //       Access: Published
00147 //  Description:
00148 ////////////////////////////////////////////////////////////////////
00149 INLINE float CollisionBox::
00150 get_radius() const {
00151   return _radius;
00152 }
00153 
00154 ////////////////////////////////////////////////////////////////////
00155 //     Function: CollisionBox::get_num_points
00156 //       Access: Published
00157 //  Description: Returns 8: the number of vertices of a rectangular solid.
00158 ////////////////////////////////////////////////////////////////////
00159 INLINE_MATHUTIL int CollisionBox::
00160 get_num_points() const {
00161   return 8;
00162 }
00163 
00164 ////////////////////////////////////////////////////////////////////
00165 //     Function: CollisionBox::get_point
00166 //       Access: Published
00167 //  Description: Returns the nth vertex of the OBB.
00168 ////////////////////////////////////////////////////////////////////
00169 INLINE_MATHUTIL LPoint3f CollisionBox::
00170 get_point(int n) const {
00171   nassertr(n >= 0 && n < 8, LPoint3f::zero());
00172   return _vertex[n];
00173 }
00174 
00175 
00176 ////////////////////////////////////////////////////////////////////
00177 //     Function: CollisionBox::get_point_aabb
00178 //       Access: Published
00179 //  Description: Returns the nth vertex of the Axis Aligned Bounding Box.
00180 ////////////////////////////////////////////////////////////////////
00181 INLINE_MATHUTIL LPoint3f CollisionBox::
00182 get_point_aabb(int n) const {
00183   nassertr(n >= 0 && n < 8, LPoint3f::zero());
00184   
00185   // We do some trickery assuming that _min and _max are consecutive
00186   // in memory.
00187   const LPoint3f *a = &_min;
00188   return LPoint3f(a[(n>>2)&1][0], a[(n>>1)&1][1], a[(n)&1][2]);
00189 }
00190 
00191 ////////////////////////////////////////////////////////////////////
00192 //     Function: CollisionBox::get_num_planes
00193 //       Access: Published
00194 //  Description: Returns 6: the number of faces of a rectangular solid.
00195 ////////////////////////////////////////////////////////////////////
00196 INLINE_MATHUTIL int CollisionBox::
00197 get_num_planes() const {
00198   return 6;
00199 }
00200 
00201 ////////////////////////////////////////////////////////////////////
00202 //     Function: CollisionBox::get_plane
00203 //       Access: Published
00204 //  Description: Returns the nth face of the rectangular solid.
00205 ////////////////////////////////////////////////////////////////////
00206 INLINE_MATHUTIL Planef CollisionBox::
00207 get_plane(int n) const {
00208   nassertr(n >= 0 && n < 6, Planef());
00209   return _planes[n];
00210 }
00211 
00212 ////////////////////////////////////////////////////////////////////
00213 //     Function: CollisionBox::set_plane
00214 //       Access: Published
00215 //  Description: Creates the nth face of the rectangular solid.
00216 ////////////////////////////////////////////////////////////////////
00217 INLINE_MATHUTIL Planef CollisionBox::
00218 set_plane(int n) const {
00219   nassertr(n >= 0 && n < 6, Planef());
00220   return Planef(get_point(plane_def[n][0]),
00221                 get_point(plane_def[n][1]),
00222                 get_point(plane_def[n][2]));
00223 }
00224 
00225 
00226 ////////////////////////////////////////////////////////////////////
00227 //     Function: CollisionBox::is_right
00228 //       Access: Private, Static
00229 //  Description: Returns true if the 2-d v1 is to the right of v2.
00230 ////////////////////////////////////////////////////////////////////
00231 INLINE bool CollisionBox::
00232 is_right(const LVector2f &v1, const LVector2f &v2) {
00233   return (v1[0] * v2[1] - v1[1] * v2[0]) > 1.0e-6f;
00234 }
00235 
00236 ////////////////////////////////////////////////////////////////////
00237 //     Function: CollisionBox::dist_to_line
00238 //       Access: Private, Static
00239 //  Description: Returns the linear distance of p to the line defined
00240 //               by f and f+v, where v is a normalized vector.  The
00241 //               result is negative if p is left of the line, positive
00242 //               if it is right of the line.
00243 ////////////////////////////////////////////////////////////////////
00244 INLINE float CollisionBox::
00245 dist_to_line(const LPoint2f &p,
00246              const LPoint2f &f, const LVector2f &v) {
00247   LVector2f v1 = (p - f);
00248   return (v1[0] * v[1] - v1[1] * v[0]);
00249 }
00250 
00251 ////////////////////////////////////////////////////////////////////
00252 //     Function: CollisionBox::to_2d
00253 //       Access: Private
00254 //  Description: Assuming the indicated point in 3-d space lies within
00255 //               the polygon's plane, returns the corresponding point
00256 //               in the polygon's 2-d definition space.
00257 ////////////////////////////////////////////////////////////////////
00258 INLINE LPoint2f CollisionBox::
00259 to_2d(const LVecBase3f &point3d, int plane) const {
00260   LPoint3f point = LPoint3f(point3d) * _to_2d_mat[plane];
00261   return LPoint2f(point[0], point[2]);
00262 }
00263 
00264 ////////////////////////////////////////////////////////////////////
00265 //     Function: CollisionBox::calc_to_3d_mat
00266 //       Access: Private
00267 //  Description: Fills the indicated matrix with the appropriate
00268 //               rotation transform to move points from the 2-d plane
00269 //               into the 3-d (X, 0, Z) plane.
00270 ////////////////////////////////////////////////////////////////////
00271 INLINE void CollisionBox::
00272 calc_to_3d_mat(LMatrix4f &to_3d_mat,int plane) const {
00273   // We have to be explicit about the coordinate system--we
00274   // specifically mean CS_zup_right, because that points the forward
00275   // vector down the Y axis and moves the coords in (X, 0, Z).  We
00276   // want this effect regardless of the user's coordinate system of
00277   // choice.
00278 
00279   // The up vector, on the other hand, is completely arbitrary.
00280 
00281   look_at(to_3d_mat, -get_plane(plane).get_normal(), 
00282           LVector3f(0.0f, 0.0f, 1.0f), CS_zup_right);
00283   to_3d_mat.set_row(3, get_plane(plane).get_point());
00284 }
00285 
00286 ////////////////////////////////////////////////////////////////////
00287 //     Function: CollisionBox::rederive_to_3d_mat
00288 //       Access: Private
00289 //  Description: Fills the indicated matrix with the appropriate
00290 //               rotation transform to move points from the 2-d plane
00291 //               into the 3-d (X, 0, Z) plane.
00292 //
00293 //               This is essentially similar to calc_to_3d_mat, except
00294 //               that the matrix is rederived from whatever is stored
00295 //               in _to_2d_mat, guaranteeing that it will match
00296 //               whatever algorithm produced that one, even if it was
00297 //               produced on a different machine with different
00298 //               numerical precision.
00299 ////////////////////////////////////////////////////////////////////
00300 INLINE void CollisionBox::
00301 rederive_to_3d_mat(LMatrix4f &to_3d_mat, int plane) const {
00302   to_3d_mat.invert_from(_to_2d_mat[plane]);
00303 }
00304 
00305 ////////////////////////////////////////////////////////////////////
00306 //     Function: CollisionBox::to_3d
00307 //       Access: Private, Static
00308 //  Description: Extrude the indicated point in the polygon's 2-d
00309 //               definition space back into 3-d coordinates.
00310 ////////////////////////////////////////////////////////////////////
00311 INLINE LPoint3f CollisionBox::
00312 to_3d(const LVecBase2f &point2d, const LMatrix4f &to_3d_mat) {
00313   return LPoint3f(point2d[0], 0.0f, point2d[1]) * to_3d_mat;
00314 }
00315 
00316 ////////////////////////////////////////////////////////////////////
00317 //     Function: CollisionBox::PointDef::Constructor
00318 //       Access: Public
00319 //  Description: 
00320 ////////////////////////////////////////////////////////////////////
00321 INLINE CollisionBox::PointDef::
00322 PointDef(const LPoint2f &p, const LVector2f &v) : _p(p), _v(v) {
00323 }
00324 
00325 ////////////////////////////////////////////////////////////////////
00326 //     Function: CollisionBox::PointDef::Constructor
00327 //       Access: Public
00328 //  Description: 
00329 ////////////////////////////////////////////////////////////////////
00330 INLINE CollisionBox::PointDef::
00331 PointDef(float x, float y) : _p(x, y), _v(0.0f, 0.0f) {
00332 }
00333 
00334 ////////////////////////////////////////////////////////////////////
00335 //     Function: CollisionBox::PointDef::Copy Constructor
00336 //       Access: Public
00337 //  Description: 
00338 ////////////////////////////////////////////////////////////////////
00339 INLINE CollisionBox::PointDef::
00340 PointDef(const CollisionBox::PointDef &copy) : _p(copy._p), _v(copy._v) {
00341 }
00342 
00343 ////////////////////////////////////////////////////////////////////
00344 //     Function: CollisionBox::PointDef::Copy Assignment Operator
00345 //       Access: Public
00346 //  Description: 
00347 ////////////////////////////////////////////////////////////////////
00348 INLINE void CollisionBox::PointDef::
00349 operator = (const CollisionBox::PointDef &copy) {
00350   _p = copy._p;
00351   _v = copy._v;
00352 }
00353 ////////////////////////////////////////////////////////////////////
00354 //     Function: CollisionBox::get_plane_points
00355 //       Access: Public
00356 //  Description: returns the points that form the nth plane
00357 ////////////////////////////////////////////////////////////////////
00358 
00359 INLINE CollisionBox::Points CollisionBox::
00360 get_plane_points( int n ){
00361   return _points[n];
00362 }
 All Classes Functions Variables Enumerations