Panda3D

collisionPlane.cxx

00001 // Filename: collisionPlane.cxx
00002 // Created by:  drose (25Apr00)
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 #include "collisionPlane.h"
00017 #include "collisionHandler.h"
00018 #include "collisionEntry.h"
00019 #include "collisionSphere.h"
00020 #include "collisionLine.h"
00021 #include "collisionRay.h"
00022 #include "collisionSegment.h"
00023 #include "config_collide.h"
00024 #include "pointerToArray.h"
00025 #include "geomNode.h"
00026 #include "geom.h"
00027 #include "datagram.h"
00028 #include "datagramIterator.h"
00029 #include "bamReader.h"
00030 #include "bamWriter.h"
00031 #include "boundingPlane.h"
00032 #include "geom.h"
00033 #include "geomTrifans.h"
00034 #include "geomLinestrips.h"
00035 #include "geomVertexWriter.h"
00036 
00037 PStatCollector CollisionPlane::_volume_pcollector("Collision Volumes:CollisionPlane");
00038 PStatCollector CollisionPlane::_test_pcollector("Collision Tests:CollisionPlane");
00039 TypeHandle CollisionPlane::_type_handle;
00040 
00041 ////////////////////////////////////////////////////////////////////
00042 //     Function: CollisionPlane::make_copy
00043 //       Access: Public, Virtual
00044 //  Description:
00045 ////////////////////////////////////////////////////////////////////
00046 CollisionSolid *CollisionPlane::
00047 make_copy() {
00048   return new CollisionPlane(*this);
00049 }
00050 
00051 ////////////////////////////////////////////////////////////////////
00052 //     Function: CollisionPlane::xform
00053 //       Access: Public, Virtual
00054 //  Description: Transforms the solid by the indicated matrix.
00055 ////////////////////////////////////////////////////////////////////
00056 void CollisionPlane::
00057 xform(const LMatrix4f &mat) {
00058   _plane = _plane * mat;
00059   CollisionSolid::xform(mat);
00060 }
00061 
00062 ////////////////////////////////////////////////////////////////////
00063 //     Function: CollisionPlane::get_collision_origin
00064 //       Access: Public, Virtual
00065 //  Description: Returns the point in space deemed to be the "origin"
00066 //               of the solid for collision purposes.  The closest
00067 //               intersection point to this origin point is considered
00068 //               to be the most significant.
00069 ////////////////////////////////////////////////////////////////////
00070 LPoint3f CollisionPlane::
00071 get_collision_origin() const {
00072   // No real sensible origin exists for a plane.  We return 0, 0, 0,
00073   // without even bothering to ensure that that point exists on the
00074   // plane.
00075   return LPoint3f::origin();
00076 }
00077 
00078 ////////////////////////////////////////////////////////////////////
00079 //     Function: CollisionPlane::get_volume_pcollector
00080 //       Access: Public, Virtual
00081 //  Description: Returns a PStatCollector that is used to count the
00082 //               number of bounding volume tests made against a solid
00083 //               of this type in a given frame.
00084 ////////////////////////////////////////////////////////////////////
00085 PStatCollector &CollisionPlane::
00086 get_volume_pcollector() {
00087   return _volume_pcollector;
00088 }
00089 
00090 ////////////////////////////////////////////////////////////////////
00091 //     Function: CollisionPlane::get_test_pcollector
00092 //       Access: Public, Virtual
00093 //  Description: Returns a PStatCollector that is used to count the
00094 //               number of intersection tests made against a solid
00095 //               of this type in a given frame.
00096 ////////////////////////////////////////////////////////////////////
00097 PStatCollector &CollisionPlane::
00098 get_test_pcollector() {
00099   return _test_pcollector;
00100 }
00101 
00102 ////////////////////////////////////////////////////////////////////
00103 //     Function: CollisionPlane::output
00104 //       Access: Public, Virtual
00105 //  Description:
00106 ////////////////////////////////////////////////////////////////////
00107 void CollisionPlane::
00108 output(ostream &out) const {
00109   out << "cplane, (" << _plane << ")";
00110 }
00111 
00112 ////////////////////////////////////////////////////////////////////
00113 //     Function: CollisionPlane::compute_internal_bounds
00114 //       Access: Protected, Virtual
00115 //  Description:
00116 ////////////////////////////////////////////////////////////////////
00117 PT(BoundingVolume) CollisionPlane::
00118 compute_internal_bounds() const {
00119   return new BoundingPlane(_plane);
00120 }
00121 
00122 ////////////////////////////////////////////////////////////////////
00123 //     Function: CollisionPlane::test_intersection_from_sphere
00124 //       Access: Public, Virtual
00125 //  Description:
00126 ////////////////////////////////////////////////////////////////////
00127 PT(CollisionEntry) CollisionPlane::
00128 test_intersection_from_sphere(const CollisionEntry &entry) const {
00129   const CollisionSphere *sphere;
00130   DCAST_INTO_R(sphere, entry.get_from(), 0);
00131 
00132   const LMatrix4f &wrt_mat = entry.get_wrt_mat();
00133 
00134   LPoint3f from_center = sphere->get_center() * wrt_mat;
00135   LVector3f from_radius_v =
00136     LVector3f(sphere->get_radius(), 0.0f, 0.0f) * wrt_mat;
00137   float from_radius = length(from_radius_v);
00138 
00139   float dist = dist_to_plane(from_center);
00140   if (dist > from_radius) {
00141     // No intersection.
00142     return NULL;
00143   }
00144 
00145   if (collide_cat.is_debug()) {
00146     collide_cat.debug()
00147       << "intersection detected from " << entry.get_from_node_path()
00148       << " into " << entry.get_into_node_path() << "\n";
00149   }
00150   PT(CollisionEntry) new_entry = new CollisionEntry(entry);
00151 
00152   LVector3f from_normal = get_normal() * entry.get_inv_wrt_mat();
00153 
00154   LVector3f normal = (
00155     has_effective_normal() && sphere->get_respect_effective_normal())
00156     ? get_effective_normal() : get_normal();
00157 
00158   new_entry->set_surface_normal(normal);
00159   new_entry->set_surface_point(from_center - get_normal() * dist);
00160   new_entry->set_interior_point(from_center - get_normal() * from_radius);
00161 
00162   return new_entry;
00163 }
00164 
00165 ////////////////////////////////////////////////////////////////////
00166 //     Function: CollisionPlane::test_intersection_from_line
00167 //       Access: Public, Virtual
00168 //  Description:
00169 ////////////////////////////////////////////////////////////////////
00170 PT(CollisionEntry) CollisionPlane::
00171 test_intersection_from_line(const CollisionEntry &entry) const {
00172   const CollisionLine *line;
00173   DCAST_INTO_R(line, entry.get_from(), 0);
00174 
00175   const LMatrix4f &wrt_mat = entry.get_wrt_mat();
00176 
00177   LPoint3f from_origin = line->get_origin() * wrt_mat;
00178   LVector3f from_direction = line->get_direction() * wrt_mat;
00179 
00180   float t;
00181   if (!_plane.intersects_line(t, from_origin, from_direction)) {
00182     // No intersection.  The line is parallel to the plane.
00183 
00184     if (_plane.dist_to_plane(from_origin) > 0.0f) {
00185       // The line is entirely in front of the plane.
00186       return NULL;
00187     }
00188 
00189     // The line is entirely behind the plane.
00190     t = 0.0f;
00191   }
00192 
00193   if (collide_cat.is_debug()) {
00194     collide_cat.debug()
00195       << "intersection detected from " << entry.get_from_node_path()
00196       << " into " << entry.get_into_node_path() << "\n";
00197   }
00198   PT(CollisionEntry) new_entry = new CollisionEntry(entry);
00199 
00200   LPoint3f into_intersection_point = from_origin + t * from_direction;
00201 
00202   LVector3f normal = 
00203     (has_effective_normal() && line->get_respect_effective_normal())
00204     ? get_effective_normal() : get_normal();
00205 
00206   new_entry->set_surface_normal(normal);
00207   new_entry->set_surface_point(into_intersection_point);
00208 
00209   return new_entry;
00210 }
00211 
00212 ////////////////////////////////////////////////////////////////////
00213 //     Function: CollisionPlane::test_intersection_from_ray
00214 //       Access: Public, Virtual
00215 //  Description:
00216 ////////////////////////////////////////////////////////////////////
00217 PT(CollisionEntry) CollisionPlane::
00218 test_intersection_from_ray(const CollisionEntry &entry) const {
00219   const CollisionRay *ray;
00220   DCAST_INTO_R(ray, entry.get_from(), 0);
00221 
00222   const LMatrix4f &wrt_mat = entry.get_wrt_mat();
00223 
00224   LPoint3f from_origin = ray->get_origin() * wrt_mat;
00225   LVector3f from_direction = ray->get_direction() * wrt_mat;
00226 
00227   float t;
00228 
00229   if (_plane.dist_to_plane(from_origin) < 0.0f) {
00230     // The origin of the ray is behind the plane, so we don't need to
00231     // test further.
00232     t = 0.0f;
00233 
00234   } else {
00235     if (!_plane.intersects_line(t, from_origin, from_direction)) {
00236       // No intersection.  The ray is parallel to the plane.
00237       return NULL;
00238     }
00239 
00240     if (t < 0.0f) {
00241       // The intersection point is before the start of the ray, and so
00242       // the ray is entirely in front of the plane.
00243       return NULL;
00244     }
00245   }
00246 
00247   if (collide_cat.is_debug()) {
00248     collide_cat.debug()
00249       << "intersection detected from " << entry.get_from_node_path()
00250       << " into " << entry.get_into_node_path() << "\n";
00251   }
00252   PT(CollisionEntry) new_entry = new CollisionEntry(entry);
00253 
00254   LPoint3f into_intersection_point = from_origin + t * from_direction;
00255 
00256   LVector3f normal =
00257     (has_effective_normal() && ray->get_respect_effective_normal()) 
00258     ? get_effective_normal() : get_normal();
00259 
00260   new_entry->set_surface_normal(normal);
00261   new_entry->set_surface_point(into_intersection_point);
00262 
00263   return new_entry;
00264 }
00265 
00266 ////////////////////////////////////////////////////////////////////
00267 //     Function: CollisionPlane::test_intersection_from_segment
00268 //       Access: Public, Virtual
00269 //  Description:
00270 ////////////////////////////////////////////////////////////////////
00271 PT(CollisionEntry) CollisionPlane::
00272 test_intersection_from_segment(const CollisionEntry &entry) const {
00273   const CollisionSegment *segment;
00274   DCAST_INTO_R(segment, entry.get_from(), 0);
00275 
00276   const LMatrix4f &wrt_mat = entry.get_wrt_mat();
00277 
00278   LPoint3f from_a = segment->get_point_a() * wrt_mat;
00279   LPoint3f from_b = segment->get_point_b() * wrt_mat;
00280   LVector3f from_direction = from_b - from_a;
00281 
00282   float t;
00283   if (_plane.dist_to_plane(from_a) < 0.0f) {
00284     // The first point of the line segment is behind the plane, so we
00285     // don't need to test further.
00286     t = 0.0f;
00287 
00288   } else {
00289     if (!_plane.intersects_line(t, from_a, from_direction)) {
00290       // No intersection.  The line segment is parallel to the plane.
00291       return NULL;
00292     }
00293 
00294     if (t < 0.0f || t > 1.0f) {
00295       // The intersection point is before the start of the segment or
00296       // after the end of the segment.  Therefore, the line segment is
00297       // entirely in front of the plane.
00298       return NULL;
00299     }
00300   }
00301 
00302   if (collide_cat.is_debug()) {
00303     collide_cat.debug()
00304       << "intersection detected from " << entry.get_from_node_path()
00305       << " into " << entry.get_into_node_path() << "\n";
00306   }
00307   PT(CollisionEntry) new_entry = new CollisionEntry(entry);
00308 
00309   LPoint3f into_intersection_point = from_a + t * from_direction;
00310 
00311   LVector3f normal = (has_effective_normal() && segment->get_respect_effective_normal()) ? get_effective_normal() : get_normal();
00312 
00313   new_entry->set_surface_normal(normal);
00314   new_entry->set_surface_point(into_intersection_point);
00315 
00316   return new_entry;
00317 }
00318 
00319 ////////////////////////////////////////////////////////////////////
00320 //     Function: CollisionPlane::test_intersection_from_parabola
00321 //       Access: Public, Virtual
00322 //  Description: This is part of the double-dispatch implementation of
00323 //               test_intersection().  It is called when the "from"
00324 //               object is a parabola.
00325 ////////////////////////////////////////////////////////////////////
00326 PT(CollisionEntry) CollisionPlane::
00327 test_intersection_from_parabola(const CollisionEntry &entry) const {
00328   const CollisionParabola *parabola;
00329   DCAST_INTO_R(parabola, entry.get_from(), 0);
00330 
00331   const LMatrix4f &wrt_mat = entry.get_wrt_mat();
00332 
00333   // Convert the parabola into local coordinate space.
00334   Parabolaf local_p(parabola->get_parabola());
00335   local_p.xform(wrt_mat);
00336 
00337   float t;
00338   if (_plane.dist_to_plane(local_p.calc_point(parabola->get_t1())) < 0.0f) {
00339     // The first point in the parabola is behind the plane, so we
00340     // don't need to test further.
00341     t = parabola->get_t1();
00342 
00343   } else {
00344     float t1, t2;
00345     if (!get_plane().intersects_parabola(t1, t2, local_p)) {
00346       // No intersection.  The infinite parabola is entirely in front
00347       // of the plane.
00348       return NULL;
00349     }
00350 
00351     if (t1 >= parabola->get_t1() && t1 <= parabola->get_t2()) {
00352       if (t2 >= parabola->get_t1() && t2 <= parabola->get_t2()) {
00353         // Both intersection points are within our segment of the
00354         // parabola.  Choose the first of the two.
00355         t = min(t1, t2);
00356       } else {
00357         // Only t1 is within our segment.
00358         t = t1;
00359       }
00360       
00361     } else if (t2 >= parabola->get_t1() && t2 <= parabola->get_t2()) {
00362       // Only t2 is within our segment.
00363       t = t2;
00364       
00365     } else {
00366       // Neither intersection point is within our segment.
00367       return NULL;
00368     }
00369   }
00370 
00371   if (collide_cat.is_debug()) {
00372     collide_cat.debug()
00373       << "intersection detected from " << entry.get_from_node_path()
00374       << " into " << entry.get_into_node_path() << "\n";
00375   }
00376   PT(CollisionEntry) new_entry = new CollisionEntry(entry);
00377 
00378   LPoint3f into_intersection_point = local_p.calc_point(t);
00379 
00380   LVector3f normal = (has_effective_normal() && parabola->get_respect_effective_normal()) ? get_effective_normal() : get_normal();
00381 
00382   new_entry->set_surface_normal(normal);
00383   new_entry->set_surface_point(into_intersection_point);
00384 
00385   return new_entry;
00386 }
00387 
00388 ////////////////////////////////////////////////////////////////////
00389 //     Function: CollisionPlane::fill_viz_geom
00390 //       Access: Protected, Virtual
00391 //  Description: Fills the _viz_geom GeomNode up with Geoms suitable
00392 //               for rendering this solid.
00393 ////////////////////////////////////////////////////////////////////
00394 void CollisionPlane::
00395 fill_viz_geom() {
00396   if (collide_cat.is_debug()) {
00397     collide_cat.debug()
00398       << "Recomputing viz for " << *this << "\n";
00399   }
00400 
00401   // Since we can't represent an infinite plane, we'll have to be
00402   // satisfied with drawing a big polygon.  Choose four points on the
00403   // plane to be the corners of the polygon.
00404 
00405   // We must choose four points fairly reasonably spread apart on
00406   // the plane.  We'll start with a center point and one corner
00407   // point, and then use cross products to find the remaining three
00408   // corners of a square.
00409 
00410   // The center point will be on the axis with the largest
00411   // coefficent.  The first corner will be diagonal in the other two
00412   // dimensions.
00413 
00414   LPoint3f cp;
00415   LVector3f p1, p2, p3, p4;
00416 
00417   LVector3f normal = get_normal();
00418   float D = _plane[3];
00419 
00420   if (fabs(normal[0]) > fabs(normal[1]) &&
00421       fabs(normal[0]) > fabs(normal[2])) {
00422     // X has the largest coefficient.
00423     cp.set(-D / normal[0], 0.0f, 0.0f);
00424     p1 = LPoint3f(-(normal[1] + normal[2] + D)/normal[0], 1.0f, 1.0f) - cp;
00425 
00426   } else if (fabs(normal[1]) > fabs(normal[2])) {
00427     // Y has the largest coefficient.
00428     cp.set(0.0f, -D / normal[1], 0.0f);
00429     p1 = LPoint3f(1.0f, -(normal[0] + normal[2] + D)/normal[1], 1.0f) - cp;
00430 
00431   } else {
00432     // Z has the largest coefficient.
00433     cp.set(0.0f, 0.0f, -D / normal[2]);
00434     p1 = LPoint3f(1.0f, 1.0f, -(normal[0] + normal[1] + D)/normal[2]) - cp;
00435   }
00436 
00437   p1.normalize();
00438   p2 = cross(normal, p1);
00439   p3 = cross(normal, p2);
00440   p4 = cross(normal, p3);
00441 
00442   static const double plane_scale = 10.0;
00443 
00444   PT(GeomVertexData) vdata = new GeomVertexData
00445     ("collision", GeomVertexFormat::get_v3(),
00446      Geom::UH_static);
00447   GeomVertexWriter vertex(vdata, InternalName::get_vertex());
00448   
00449   vertex.add_data3f(cp + p1 * plane_scale);
00450   vertex.add_data3f(cp + p2 * plane_scale);
00451   vertex.add_data3f(cp + p3 * plane_scale);
00452   vertex.add_data3f(cp + p4 * plane_scale);
00453   
00454   PT(GeomTrifans) body = new GeomTrifans(Geom::UH_static);
00455   body->add_consecutive_vertices(0, 4);
00456   body->close_primitive();
00457   
00458   PT(GeomLinestrips) border = new GeomLinestrips(Geom::UH_static);
00459   border->add_consecutive_vertices(0, 4);
00460   border->add_vertex(0);
00461   border->close_primitive();
00462   
00463   PT(Geom) geom1 = new Geom(vdata);
00464   geom1->add_primitive(body);
00465   
00466   PT(Geom) geom2 = new Geom(vdata);
00467   geom2->add_primitive(border);
00468   
00469   _viz_geom->add_geom(geom1, get_solid_viz_state());
00470   _viz_geom->add_geom(geom2, get_wireframe_viz_state());
00471   
00472   _bounds_viz_geom->add_geom(geom1, get_solid_bounds_viz_state());
00473   _bounds_viz_geom->add_geom(geom2, get_wireframe_bounds_viz_state());
00474 }
00475 
00476 ////////////////////////////////////////////////////////////////////
00477 //     Function: CollisionPlane::write_datagram
00478 //       Access: Public
00479 //  Description: Function to write the important information in
00480 //               the particular object to a Datagram
00481 ////////////////////////////////////////////////////////////////////
00482 void CollisionPlane::
00483 write_datagram(BamWriter *manager, Datagram &me)
00484 {
00485   CollisionSolid::write_datagram(manager, me);
00486   _plane.write_datagram(me);
00487 }
00488 
00489 ////////////////////////////////////////////////////////////////////
00490 //     Function: CollisionPlane::fillin
00491 //       Access: Protected
00492 //  Description: Function that reads out of the datagram (or asks
00493 //               manager to read) all of the data that is needed to
00494 //               re-create this object and stores it in the appropiate
00495 //               place
00496 ////////////////////////////////////////////////////////////////////
00497 void CollisionPlane::
00498 fillin(DatagramIterator& scan, BamReader* manager)
00499 {
00500   CollisionSolid::fillin(scan, manager);
00501   _plane.read_datagram(scan);
00502 }
00503 
00504 ////////////////////////////////////////////////////////////////////
00505 //     Function: CollisionPlane::make_CollisionPlane
00506 //       Access: Protected
00507 //  Description: Factory method to generate a CollisionPlane object
00508 ////////////////////////////////////////////////////////////////////
00509 TypedWritable* CollisionPlane::
00510 make_CollisionPlane(const FactoryParams &params)
00511 {
00512   CollisionPlane *me = new CollisionPlane;
00513   DatagramIterator scan;
00514   BamReader *manager;
00515 
00516   parse_params(params, scan, manager);
00517   me->fillin(scan, manager);
00518   return me;
00519 }
00520 
00521 ////////////////////////////////////////////////////////////////////
00522 //     Function: CollisionPlane::register_with_factory
00523 //       Access: Public, Static
00524 //  Description: Factory method to generate a CollisionPlane object
00525 ////////////////////////////////////////////////////////////////////
00526 void CollisionPlane::
00527 register_with_read_factory()
00528 {
00529   BamReader::get_factory()->register_factory(get_class_type(), make_CollisionPlane);
00530 }
 All Classes Functions Variables Enumerations