Panda3D
|
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 ¶ms) { 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