Panda3D
 All Classes Functions Variables Enumerations
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 LMatrix4 &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 LPoint3 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 LPoint3::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 LMatrix4 &wrt_mat = entry.get_wrt_mat();
00133 
00134   LPoint3 from_center = sphere->get_center() * wrt_mat;
00135   LVector3 from_radius_v =
00136     LVector3(sphere->get_radius(), 0.0f, 0.0f) * wrt_mat;
00137   PN_stdfloat from_radius = length(from_radius_v);
00138 
00139   PN_stdfloat 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   LVector3 normal = (
00153     has_effective_normal() && sphere->get_respect_effective_normal())
00154     ? get_effective_normal() : get_normal();
00155 
00156   new_entry->set_surface_normal(normal);
00157   new_entry->set_surface_point(from_center - get_normal() * dist);
00158   new_entry->set_interior_point(from_center - get_normal() * from_radius);
00159 
00160   return new_entry;
00161 }
00162 
00163 ////////////////////////////////////////////////////////////////////
00164 //     Function: CollisionPlane::test_intersection_from_line
00165 //       Access: Public, Virtual
00166 //  Description:
00167 ////////////////////////////////////////////////////////////////////
00168 PT(CollisionEntry) CollisionPlane::
00169 test_intersection_from_line(const CollisionEntry &entry) const {
00170   const CollisionLine *line;
00171   DCAST_INTO_R(line, entry.get_from(), 0);
00172 
00173   const LMatrix4 &wrt_mat = entry.get_wrt_mat();
00174 
00175   LPoint3 from_origin = line->get_origin() * wrt_mat;
00176   LVector3 from_direction = line->get_direction() * wrt_mat;
00177 
00178   PN_stdfloat t;
00179   if (!_plane.intersects_line(t, from_origin, from_direction)) {
00180     // No intersection.  The line is parallel to the plane.
00181 
00182     if (_plane.dist_to_plane(from_origin) > 0.0f) {
00183       // The line is entirely in front of the plane.
00184       return NULL;
00185     }
00186 
00187     // The line is entirely behind the plane.
00188     t = 0.0f;
00189   }
00190 
00191   if (collide_cat.is_debug()) {
00192     collide_cat.debug()
00193       << "intersection detected from " << entry.get_from_node_path()
00194       << " into " << entry.get_into_node_path() << "\n";
00195   }
00196   PT(CollisionEntry) new_entry = new CollisionEntry(entry);
00197 
00198   LPoint3 into_intersection_point = from_origin + t * from_direction;
00199 
00200   LVector3 normal = 
00201     (has_effective_normal() && line->get_respect_effective_normal())
00202     ? get_effective_normal() : get_normal();
00203 
00204   new_entry->set_surface_normal(normal);
00205   new_entry->set_surface_point(into_intersection_point);
00206 
00207   return new_entry;
00208 }
00209 
00210 ////////////////////////////////////////////////////////////////////
00211 //     Function: CollisionPlane::test_intersection_from_ray
00212 //       Access: Public, Virtual
00213 //  Description:
00214 ////////////////////////////////////////////////////////////////////
00215 PT(CollisionEntry) CollisionPlane::
00216 test_intersection_from_ray(const CollisionEntry &entry) const {
00217   const CollisionRay *ray;
00218   DCAST_INTO_R(ray, entry.get_from(), 0);
00219 
00220   const LMatrix4 &wrt_mat = entry.get_wrt_mat();
00221 
00222   LPoint3 from_origin = ray->get_origin() * wrt_mat;
00223   LVector3 from_direction = ray->get_direction() * wrt_mat;
00224 
00225   PN_stdfloat t;
00226 
00227   if (_plane.dist_to_plane(from_origin) < 0.0f) {
00228     // The origin of the ray is behind the plane, so we don't need to
00229     // test further.
00230     t = 0.0f;
00231 
00232   } else {
00233     if (!_plane.intersects_line(t, from_origin, from_direction)) {
00234       // No intersection.  The ray is parallel to the plane.
00235       return NULL;
00236     }
00237 
00238     if (t < 0.0f) {
00239       // The intersection point is before the start of the ray, and so
00240       // the ray is entirely in front of the plane.
00241       return NULL;
00242     }
00243   }
00244 
00245   if (collide_cat.is_debug()) {
00246     collide_cat.debug()
00247       << "intersection detected from " << entry.get_from_node_path()
00248       << " into " << entry.get_into_node_path() << "\n";
00249   }
00250   PT(CollisionEntry) new_entry = new CollisionEntry(entry);
00251 
00252   LPoint3 into_intersection_point = from_origin + t * from_direction;
00253 
00254   LVector3 normal =
00255     (has_effective_normal() && ray->get_respect_effective_normal()) 
00256     ? get_effective_normal() : get_normal();
00257 
00258   new_entry->set_surface_normal(normal);
00259   new_entry->set_surface_point(into_intersection_point);
00260 
00261   return new_entry;
00262 }
00263 
00264 ////////////////////////////////////////////////////////////////////
00265 //     Function: CollisionPlane::test_intersection_from_segment
00266 //       Access: Public, Virtual
00267 //  Description:
00268 ////////////////////////////////////////////////////////////////////
00269 PT(CollisionEntry) CollisionPlane::
00270 test_intersection_from_segment(const CollisionEntry &entry) const {
00271   const CollisionSegment *segment;
00272   DCAST_INTO_R(segment, entry.get_from(), 0);
00273 
00274   const LMatrix4 &wrt_mat = entry.get_wrt_mat();
00275 
00276   LPoint3 from_a = segment->get_point_a() * wrt_mat;
00277   LPoint3 from_b = segment->get_point_b() * wrt_mat;
00278 
00279   PN_stdfloat dist_a = _plane.dist_to_plane(from_a);
00280   PN_stdfloat dist_b = _plane.dist_to_plane(from_b);
00281 
00282   if (dist_a >= 0.0f && dist_b >= 0.0f) {
00283     // Entirely in front of the plane means no intersection.
00284     return NULL;
00285   }
00286 
00287   if (collide_cat.is_debug()) {
00288     collide_cat.debug()
00289       << "intersection detected from " << entry.get_from_node_path()
00290       << " into " << entry.get_into_node_path() << "\n";
00291   }
00292   PT(CollisionEntry) new_entry = new CollisionEntry(entry);
00293 
00294   LVector3 normal = (has_effective_normal() && segment->get_respect_effective_normal()) ? get_effective_normal() : get_normal();
00295   new_entry->set_surface_normal(normal);
00296 
00297   PN_stdfloat t;
00298   LVector3 from_direction = from_b - from_a;
00299   if (_plane.intersects_line(t, from_a, from_direction)) {
00300     // It intersects the plane.
00301     if (t >= 0.0f && t <= 1.0f) {
00302       // Within the segment!  Yay, that means we have a surface point.
00303       new_entry->set_surface_point(from_a + t * from_direction);
00304     }
00305   }
00306 
00307   if (dist_a < dist_b) {
00308     // Point A penetrates deeper.
00309     new_entry->set_interior_point(from_a);
00310 
00311   } else if (dist_b < dist_a) {
00312     // No, point B does.
00313     new_entry->set_interior_point(from_b);
00314 
00315   } else {
00316     // Let's be fair and choose the center of the segment.
00317     new_entry->set_interior_point((from_a + from_b) * 0.5);
00318   }
00319 
00320   return new_entry;
00321 }
00322 
00323 ////////////////////////////////////////////////////////////////////
00324 //     Function: CollisionPlane::test_intersection_from_parabola
00325 //       Access: Public, Virtual
00326 //  Description: This is part of the double-dispatch implementation of
00327 //               test_intersection().  It is called when the "from"
00328 //               object is a parabola.
00329 ////////////////////////////////////////////////////////////////////
00330 PT(CollisionEntry) CollisionPlane::
00331 test_intersection_from_parabola(const CollisionEntry &entry) const {
00332   const CollisionParabola *parabola;
00333   DCAST_INTO_R(parabola, entry.get_from(), 0);
00334 
00335   const LMatrix4 &wrt_mat = entry.get_wrt_mat();
00336 
00337   // Convert the parabola into local coordinate space.
00338   LParabola local_p(parabola->get_parabola());
00339   local_p.xform(wrt_mat);
00340 
00341   PN_stdfloat t;
00342   if (_plane.dist_to_plane(local_p.calc_point(parabola->get_t1())) < 0.0f) {
00343     // The first point in the parabola is behind the plane, so we
00344     // don't need to test further.
00345     t = parabola->get_t1();
00346 
00347   } else {
00348     PN_stdfloat t1, t2;
00349     if (!get_plane().intersects_parabola(t1, t2, local_p)) {
00350       // No intersection.  The infinite parabola is entirely in front
00351       // of the plane.
00352       return NULL;
00353     }
00354 
00355     if (t1 >= parabola->get_t1() && t1 <= parabola->get_t2()) {
00356       if (t2 >= parabola->get_t1() && t2 <= parabola->get_t2()) {
00357         // Both intersection points are within our segment of the
00358         // parabola.  Choose the first of the two.
00359         t = min(t1, t2);
00360       } else {
00361         // Only t1 is within our segment.
00362         t = t1;
00363       }
00364       
00365     } else if (t2 >= parabola->get_t1() && t2 <= parabola->get_t2()) {
00366       // Only t2 is within our segment.
00367       t = t2;
00368       
00369     } else {
00370       // Neither intersection point is within our segment.
00371       return NULL;
00372     }
00373   }
00374 
00375   if (collide_cat.is_debug()) {
00376     collide_cat.debug()
00377       << "intersection detected from " << entry.get_from_node_path()
00378       << " into " << entry.get_into_node_path() << "\n";
00379   }
00380   PT(CollisionEntry) new_entry = new CollisionEntry(entry);
00381 
00382   LPoint3 into_intersection_point = local_p.calc_point(t);
00383 
00384   LVector3 normal = (has_effective_normal() && parabola->get_respect_effective_normal()) ? get_effective_normal() : get_normal();
00385 
00386   new_entry->set_surface_normal(normal);
00387   new_entry->set_surface_point(into_intersection_point);
00388 
00389   return new_entry;
00390 }
00391 
00392 ////////////////////////////////////////////////////////////////////
00393 //     Function: CollisionPlane::fill_viz_geom
00394 //       Access: Protected, Virtual
00395 //  Description: Fills the _viz_geom GeomNode up with Geoms suitable
00396 //               for rendering this solid.
00397 ////////////////////////////////////////////////////////////////////
00398 void CollisionPlane::
00399 fill_viz_geom() {
00400   if (collide_cat.is_debug()) {
00401     collide_cat.debug()
00402       << "Recomputing viz for " << *this << "\n";
00403   }
00404 
00405   // Since we can't represent an infinite plane, we'll have to be
00406   // satisfied with drawing a big polygon.  Choose four points on the
00407   // plane to be the corners of the polygon.
00408 
00409   // We must choose four points fairly reasonably spread apart on
00410   // the plane.  We'll start with a center point and one corner
00411   // point, and then use cross products to find the remaining three
00412   // corners of a square.
00413 
00414   // The center point will be on the axis with the largest
00415   // coefficent.  The first corner will be diagonal in the other two
00416   // dimensions.
00417 
00418   LPoint3 cp;
00419   LVector3 p1, p2, p3, p4;
00420 
00421   LVector3 normal = get_normal();
00422   PN_stdfloat D = _plane[3];
00423 
00424   if (fabs(normal[0]) > fabs(normal[1]) &&
00425       fabs(normal[0]) > fabs(normal[2])) {
00426     // X has the largest coefficient.
00427     cp.set(-D / normal[0], 0.0f, 0.0f);
00428     p1 = LPoint3(-(normal[1] + normal[2] + D)/normal[0], 1.0f, 1.0f) - cp;
00429 
00430   } else if (fabs(normal[1]) > fabs(normal[2])) {
00431     // Y has the largest coefficient.
00432     cp.set(0.0f, -D / normal[1], 0.0f);
00433     p1 = LPoint3(1.0f, -(normal[0] + normal[2] + D)/normal[1], 1.0f) - cp;
00434 
00435   } else {
00436     // Z has the largest coefficient.
00437     cp.set(0.0f, 0.0f, -D / normal[2]);
00438     p1 = LPoint3(1.0f, 1.0f, -(normal[0] + normal[1] + D)/normal[2]) - cp;
00439   }
00440 
00441   p1.normalize();
00442   p2 = cross(normal, p1);
00443   p3 = cross(normal, p2);
00444   p4 = cross(normal, p3);
00445 
00446   static const double plane_scale = 10.0;
00447 
00448   PT(GeomVertexData) vdata = new GeomVertexData
00449     ("collision", GeomVertexFormat::get_v3(),
00450      Geom::UH_static);
00451   GeomVertexWriter vertex(vdata, InternalName::get_vertex());
00452   
00453   vertex.add_data3(cp + p1 * plane_scale);
00454   vertex.add_data3(cp + p2 * plane_scale);
00455   vertex.add_data3(cp + p3 * plane_scale);
00456   vertex.add_data3(cp + p4 * plane_scale);
00457   
00458   PT(GeomTrifans) body = new GeomTrifans(Geom::UH_static);
00459   body->add_consecutive_vertices(0, 4);
00460   body->close_primitive();
00461   
00462   PT(GeomLinestrips) border = new GeomLinestrips(Geom::UH_static);
00463   border->add_consecutive_vertices(0, 4);
00464   border->add_vertex(0);
00465   border->close_primitive();
00466   
00467   PT(Geom) geom1 = new Geom(vdata);
00468   geom1->add_primitive(body);
00469   
00470   PT(Geom) geom2 = new Geom(vdata);
00471   geom2->add_primitive(border);
00472   
00473   _viz_geom->add_geom(geom1, get_solid_viz_state());
00474   _viz_geom->add_geom(geom2, get_wireframe_viz_state());
00475   
00476   _bounds_viz_geom->add_geom(geom1, get_solid_bounds_viz_state());
00477   _bounds_viz_geom->add_geom(geom2, get_wireframe_bounds_viz_state());
00478 }
00479 
00480 ////////////////////////////////////////////////////////////////////
00481 //     Function: CollisionPlane::write_datagram
00482 //       Access: Public
00483 //  Description: Function to write the important information in
00484 //               the particular object to a Datagram
00485 ////////////////////////////////////////////////////////////////////
00486 void CollisionPlane::
00487 write_datagram(BamWriter *manager, Datagram &me)
00488 {
00489   CollisionSolid::write_datagram(manager, me);
00490   _plane.write_datagram(me);
00491 }
00492 
00493 ////////////////////////////////////////////////////////////////////
00494 //     Function: CollisionPlane::fillin
00495 //       Access: Protected
00496 //  Description: Function that reads out of the datagram (or asks
00497 //               manager to read) all of the data that is needed to
00498 //               re-create this object and stores it in the appropiate
00499 //               place
00500 ////////////////////////////////////////////////////////////////////
00501 void CollisionPlane::
00502 fillin(DatagramIterator& scan, BamReader* manager)
00503 {
00504   CollisionSolid::fillin(scan, manager);
00505   _plane.read_datagram(scan);
00506 }
00507 
00508 ////////////////////////////////////////////////////////////////////
00509 //     Function: CollisionPlane::make_CollisionPlane
00510 //       Access: Protected
00511 //  Description: Factory method to generate a CollisionPlane object
00512 ////////////////////////////////////////////////////////////////////
00513 TypedWritable* CollisionPlane::
00514 make_CollisionPlane(const FactoryParams &params)
00515 {
00516   CollisionPlane *me = new CollisionPlane;
00517   DatagramIterator scan;
00518   BamReader *manager;
00519 
00520   parse_params(params, scan, manager);
00521   me->fillin(scan, manager);
00522   return me;
00523 }
00524 
00525 ////////////////////////////////////////////////////////////////////
00526 //     Function: CollisionPlane::register_with_factory
00527 //       Access: Public, Static
00528 //  Description: Factory method to generate a CollisionPlane object
00529 ////////////////////////////////////////////////////////////////////
00530 void CollisionPlane::
00531 register_with_read_factory()
00532 {
00533   BamReader::get_factory()->register_factory(get_class_type(), make_CollisionPlane);
00534 }
 All Classes Functions Variables Enumerations