Panda3D

collisionInvSphere.cxx

00001 // Filename: collisionInvSphere.cxx
00002 // Created by:  drose (05Jan05)
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 #include "collisionInvSphere.h"
00016 #include "collisionSphere.h"
00017 #include "collisionLine.h"
00018 #include "collisionRay.h"
00019 #include "collisionSegment.h"
00020 #include "collisionHandler.h"
00021 #include "collisionEntry.h"
00022 #include "config_collide.h"
00023 #include "omniBoundingVolume.h"
00024 #include "datagram.h"
00025 #include "datagramIterator.h"
00026 #include "bamReader.h"
00027 #include "bamWriter.h"
00028 #include "nearly_zero.h"
00029 #include "geom.h"
00030 #include "geomTristrips.h"
00031 #include "geomVertexWriter.h"
00032 
00033 PStatCollector CollisionInvSphere::_volume_pcollector("Collision Volumes:CollisionInvSphere");
00034 PStatCollector CollisionInvSphere::_test_pcollector("Collision Tests:CollisionInvSphere");
00035 TypeHandle CollisionInvSphere::_type_handle;
00036 
00037 ////////////////////////////////////////////////////////////////////
00038 //     Function: CollisionInvSphere::make_copy
00039 //       Access: Public, Virtual
00040 //  Description:
00041 ////////////////////////////////////////////////////////////////////
00042 CollisionSolid *CollisionInvSphere::
00043 make_copy() {
00044   return new CollisionInvSphere(*this);
00045 }
00046 
00047 ////////////////////////////////////////////////////////////////////
00048 //     Function: CollisionInvSphere::test_intersection
00049 //       Access: Public, Virtual
00050 //  Description:
00051 ////////////////////////////////////////////////////////////////////
00052 PT(CollisionEntry) CollisionInvSphere::
00053 test_intersection(const CollisionEntry &) const {
00054   report_undefined_from_intersection(get_type());
00055   return NULL;
00056 }
00057 
00058 ////////////////////////////////////////////////////////////////////
00059 //     Function: CollisionInvSphere::get_volume_pcollector
00060 //       Access: Public, Virtual
00061 //  Description: Returns a PStatCollector that is used to count the
00062 //               number of bounding volume tests made against a solid
00063 //               of this type in a given frame.
00064 ////////////////////////////////////////////////////////////////////
00065 PStatCollector &CollisionInvSphere::
00066 get_volume_pcollector() {
00067   return _volume_pcollector;
00068 }
00069 
00070 ////////////////////////////////////////////////////////////////////
00071 //     Function: CollisionInvSphere::get_test_pcollector
00072 //       Access: Public, Virtual
00073 //  Description: Returns a PStatCollector that is used to count the
00074 //               number of intersection tests made against a solid
00075 //               of this type in a given frame.
00076 ////////////////////////////////////////////////////////////////////
00077 PStatCollector &CollisionInvSphere::
00078 get_test_pcollector() {
00079   return _test_pcollector;
00080 }
00081 
00082 ////////////////////////////////////////////////////////////////////
00083 //     Function: CollisionInvSphere::output
00084 //       Access: Public, Virtual
00085 //  Description:
00086 ////////////////////////////////////////////////////////////////////
00087 void CollisionInvSphere::
00088 output(ostream &out) const {
00089   out << "invsphere, c (" << get_center() << "), r " << get_radius();
00090 }
00091 
00092 ////////////////////////////////////////////////////////////////////
00093 //     Function: CollisionInvSphere::compute_internal_bounds
00094 //       Access: Protected, Virtual
00095 //  Description:
00096 ////////////////////////////////////////////////////////////////////
00097 PT(BoundingVolume) CollisionInvSphere::
00098 compute_internal_bounds() const {
00099   // An inverse sphere always has an infinite bounding volume, since
00100   // everything outside the sphere is solid matter.
00101   return new OmniBoundingVolume();
00102 }
00103 
00104 ////////////////////////////////////////////////////////////////////
00105 //     Function: CollisionInvSphere::test_intersection_from_sphere
00106 //       Access: Public, Virtual
00107 //  Description:
00108 ////////////////////////////////////////////////////////////////////
00109 PT(CollisionEntry) CollisionInvSphere::
00110 test_intersection_from_sphere(const CollisionEntry &entry) const {
00111   const CollisionSphere *sphere;
00112   DCAST_INTO_R(sphere, entry.get_from(), 0);
00113 
00114   const LMatrix4 &wrt_mat = entry.get_wrt_mat();
00115 
00116   LPoint3 from_center = sphere->get_center() * wrt_mat;
00117   LVector3 from_radius_v =
00118     LVector3(sphere->get_radius(), 0.0f, 0.0f) * wrt_mat;
00119   PN_stdfloat from_radius = length(from_radius_v);
00120 
00121   LPoint3 into_center = get_center();
00122   PN_stdfloat into_radius = get_radius();
00123 
00124   LVector3 vec = from_center - into_center;
00125   PN_stdfloat dist2 = dot(vec, vec);
00126   if (dist2 < (into_radius - from_radius) * (into_radius - from_radius)) {
00127     // No intersection--the sphere is within the hollow.
00128     return NULL;
00129   }
00130 
00131   if (collide_cat.is_debug()) {
00132     collide_cat.debug()
00133       << "intersection detected from " << entry.get_from_node_path()
00134       << " into " << entry.get_into_node_path() << "\n";
00135   }
00136   PT(CollisionEntry) new_entry = new CollisionEntry(entry);
00137 
00138   LVector3 surface_normal;
00139   PN_stdfloat vec_length = vec.length();
00140   if (IS_NEARLY_ZERO(vec_length)) {
00141     // If we don't have a collision normal (e.g. the centers are
00142     // exactly coincident), then make up an arbitrary normal--any one
00143     // is as good as any other.
00144     surface_normal.set(1.0, 0.0, 0.0);
00145   } else {
00146     surface_normal = vec / -vec_length;
00147   }
00148 
00149   LVector3 normal = (has_effective_normal() && sphere->get_respect_effective_normal()) ? get_effective_normal() : surface_normal;
00150 
00151   new_entry->set_surface_normal(normal);
00152   new_entry->set_surface_point(into_center - surface_normal * into_radius);
00153   new_entry->set_interior_point(from_center - surface_normal * from_radius);
00154 
00155   return new_entry;
00156 }
00157 
00158 ////////////////////////////////////////////////////////////////////
00159 //     Function: CollisionInvSphere::test_intersection_from_line
00160 //       Access: Public, Virtual
00161 //  Description:
00162 ////////////////////////////////////////////////////////////////////
00163 PT(CollisionEntry) CollisionInvSphere::
00164 test_intersection_from_line(const CollisionEntry &entry) const {
00165   const CollisionLine *line;
00166   DCAST_INTO_R(line, entry.get_from(), 0);
00167 
00168   const LMatrix4 &wrt_mat = entry.get_wrt_mat();
00169 
00170   LPoint3 from_origin = line->get_origin() * wrt_mat;
00171   LVector3 from_direction = line->get_direction() * wrt_mat;
00172 
00173   double t1, t2;
00174   if (!intersects_line(t1, t2, from_origin, from_direction, 0.0f)) {
00175     // The line is in the middle of space, and therefore intersects
00176     // the sphere.
00177     t1 = t2 = 0.0;
00178   }
00179 
00180   if (collide_cat.is_debug()) {
00181     collide_cat.debug()
00182       << "intersection detected from " << entry.get_from_node_path()
00183       << " into " << entry.get_into_node_path() << "\n";
00184   }
00185   PT(CollisionEntry) new_entry = new CollisionEntry(entry);
00186 
00187   LPoint3 into_intersection_point = from_origin + t2 * from_direction;
00188   new_entry->set_surface_point(into_intersection_point);
00189 
00190   if (has_effective_normal() && line->get_respect_effective_normal()) {
00191     new_entry->set_surface_normal(get_effective_normal());
00192   } else {
00193     LVector3 normal = into_intersection_point - get_center();
00194     normal.normalize();
00195     new_entry->set_surface_normal(-normal);
00196   }
00197 
00198   return new_entry;
00199 }
00200 
00201 ////////////////////////////////////////////////////////////////////
00202 //     Function: CollisionInvSphere::test_intersection_from_ray
00203 //       Access: Public, Virtual
00204 //  Description:
00205 ////////////////////////////////////////////////////////////////////
00206 PT(CollisionEntry) CollisionInvSphere::
00207 test_intersection_from_ray(const CollisionEntry &entry) const {
00208   const CollisionRay *ray;
00209   DCAST_INTO_R(ray, entry.get_from(), 0);
00210 
00211   const LMatrix4 &wrt_mat = entry.get_wrt_mat();
00212 
00213   LPoint3 from_origin = ray->get_origin() * wrt_mat;
00214   LVector3 from_direction = ray->get_direction() * wrt_mat;
00215 
00216   double t1, t2;
00217   if (!intersects_line(t1, t2, from_origin, from_direction, 0.0f)) {
00218     // The ray is in the middle of space, and therefore intersects
00219     // the sphere.
00220     t1 = t2 = 0.0;
00221   }
00222 
00223   t2 = max(t2, 0.0);
00224 
00225   if (collide_cat.is_debug()) {
00226     collide_cat.debug()
00227       << "intersection detected from " << entry.get_from_node_path()
00228       << " into " << entry.get_into_node_path() << "\n";
00229   }
00230   PT(CollisionEntry) new_entry = new CollisionEntry(entry);
00231 
00232   LPoint3 into_intersection_point;
00233   into_intersection_point = from_origin + t2 * from_direction;
00234   new_entry->set_surface_point(into_intersection_point);
00235 
00236   if (has_effective_normal() && ray->get_respect_effective_normal()) {
00237     new_entry->set_surface_normal(get_effective_normal());
00238   } else {
00239     LVector3 normal = into_intersection_point - get_center();
00240     normal.normalize();
00241     new_entry->set_surface_normal(-normal);
00242   }
00243 
00244   return new_entry;
00245 }
00246 
00247 ////////////////////////////////////////////////////////////////////
00248 //     Function: CollisionInvSphere::test_intersection_from_segment
00249 //       Access: Public, Virtual
00250 //  Description:
00251 ////////////////////////////////////////////////////////////////////
00252 PT(CollisionEntry) CollisionInvSphere::
00253 test_intersection_from_segment(const CollisionEntry &entry) const {
00254   const CollisionSegment *segment;
00255   DCAST_INTO_R(segment, entry.get_from(), 0);
00256 
00257   const LMatrix4 &wrt_mat = entry.get_wrt_mat();
00258 
00259   LPoint3 from_a = segment->get_point_a() * wrt_mat;
00260   LPoint3 from_b = segment->get_point_b() * wrt_mat;
00261   LVector3 from_direction = from_b - from_a;
00262 
00263   double t1, t2;
00264   if (!intersects_line(t1, t2, from_a, from_direction, 0.0f)) {
00265     // The segment is in the middle of space, and therefore intersects
00266     // the sphere.
00267     t1 = t2 = 0.0;
00268   }
00269   
00270   double t;
00271   if (t2 <= 0.0) {
00272     // The segment is completely below the shell.
00273     t = 0.0;
00274 
00275   } else if (t1 >= 1.0) {
00276     // The segment is completely above the shell.
00277     t = 1.0;
00278 
00279   } else if (t2 <= 1.0) {
00280     // The bottom edge of the segment intersects the shell.
00281     t = min(t2, 1.0);
00282 
00283   } else if (t1 >= 0.0) {
00284     // The top edge of the segment intersects the shell.
00285     t = max(t1, 0.0);
00286 
00287   } else {
00288     // Neither edge of the segment intersects the shell.  It follows
00289     // that both intersection points are within the hollow center of
00290     // the sphere; therefore, there is no intersection.
00291     return NULL;
00292   }
00293 
00294   if (collide_cat.is_debug()) {
00295     collide_cat.debug()
00296       << "intersection detected from " << entry.get_from_node_path()
00297       << " into " << entry.get_into_node_path() << "\n";
00298   }
00299   PT(CollisionEntry) new_entry = new CollisionEntry(entry);
00300 
00301   LPoint3 into_intersection_point = from_a + t * from_direction;
00302   new_entry->set_surface_point(into_intersection_point);
00303 
00304   if (has_effective_normal() && segment->get_respect_effective_normal()) {
00305     new_entry->set_surface_normal(get_effective_normal());
00306   } else {
00307     LVector3 normal = into_intersection_point - get_center();
00308     normal.normalize();
00309     new_entry->set_surface_normal(-normal);
00310   }
00311 
00312   return new_entry;
00313 }
00314 
00315 ////////////////////////////////////////////////////////////////////
00316 //     Function: CollisionInvSphere::fill_viz_geom
00317 //       Access: Protected, Virtual
00318 //  Description: Fills the _viz_geom GeomNode up with Geoms suitable
00319 //               for rendering this solid.
00320 ////////////////////////////////////////////////////////////////////
00321 void CollisionInvSphere::
00322 fill_viz_geom() {
00323   if (collide_cat.is_debug()) {
00324     collide_cat.debug()
00325       << "Recomputing viz for " << *this << "\n";
00326   }
00327 
00328   static const int num_slices = 16;
00329   static const int num_stacks = 8;
00330 
00331   PT(GeomVertexData) vdata = new GeomVertexData
00332     ("collision", GeomVertexFormat::get_v3(),
00333      Geom::UH_static);
00334   GeomVertexWriter vertex(vdata, InternalName::get_vertex());
00335   
00336   PT(GeomTristrips) strip = new GeomTristrips(Geom::UH_static);
00337   for (int sl = 0; sl < num_slices; ++sl) {
00338     PN_stdfloat longitude0 = (PN_stdfloat)sl / (PN_stdfloat)num_slices;
00339     PN_stdfloat longitude1 = (PN_stdfloat)(sl + 1) / (PN_stdfloat)num_slices;
00340     vertex.add_data3(compute_point(0.0, longitude0));
00341     for (int st = 1; st < num_stacks; ++st) {
00342       PN_stdfloat latitude = (PN_stdfloat)st / (PN_stdfloat)num_stacks;
00343       vertex.add_data3(compute_point(latitude, longitude1));
00344       vertex.add_data3(compute_point(latitude, longitude0));
00345     }
00346     vertex.add_data3(compute_point(1.0, longitude0));
00347     
00348     strip->add_next_vertices(num_stacks * 2);
00349     strip->close_primitive();
00350   }
00351   
00352   PT(Geom) geom = new Geom(vdata);
00353   geom->add_primitive(strip);
00354   
00355   _viz_geom->add_geom(geom, get_solid_viz_state());
00356 }
00357 
00358 ////////////////////////////////////////////////////////////////////
00359 //     Function: CollisionInvSphere::register_with_read_factory
00360 //       Access: Public, Static
00361 //  Description: Factory method to generate a CollisionInvSphere object
00362 ////////////////////////////////////////////////////////////////////
00363 void CollisionInvSphere::
00364 register_with_read_factory() {
00365   BamReader::get_factory()->register_factory(get_class_type(), make_CollisionInvSphere);
00366 }
00367 
00368 ////////////////////////////////////////////////////////////////////
00369 //     Function: CollisionInvSphere::write_datagram
00370 //       Access: Public
00371 //  Description: Function to write the important information in
00372 //               the particular object to a Datagram
00373 ////////////////////////////////////////////////////////////////////
00374 void CollisionInvSphere::
00375 write_datagram(BamWriter *manager, Datagram &me) {
00376   CollisionSphere::write_datagram(manager, me);
00377 }
00378 
00379 ////////////////////////////////////////////////////////////////////
00380 //     Function: CollisionInvSphere::make_CollisionInvSphere
00381 //       Access: Protected
00382 //  Description: Factory method to generate a CollisionInvSphere object
00383 ////////////////////////////////////////////////////////////////////
00384 TypedWritable *CollisionInvSphere::
00385 make_CollisionInvSphere(const FactoryParams &params) {
00386   CollisionInvSphere *me = new CollisionInvSphere;
00387   DatagramIterator scan;
00388   BamReader *manager;
00389 
00390   parse_params(params, scan, manager);
00391   me->fillin(scan, manager);
00392   return me;
00393 }
00394 
00395 ////////////////////////////////////////////////////////////////////
00396 //     Function: CollisionInvSphere::fillin
00397 //       Access: Protected
00398 //  Description: Function that reads out of the datagram (or asks
00399 //               manager to read) all of the data that is needed to
00400 //               re-create this object and stores it in the appropiate
00401 //               place
00402 ////////////////////////////////////////////////////////////////////
00403 void CollisionInvSphere::
00404 fillin(DatagramIterator& scan, BamReader* manager) {
00405   CollisionSphere::fillin(scan, manager);
00406 }
00407 
 All Classes Functions Variables Enumerations