00001
00002
00003
00004
00005
00006
00007
00008
00009
00010
00011
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
00039
00040
00041
00042 CollisionSolid *CollisionInvSphere::
00043 make_copy() {
00044 return new CollisionInvSphere(*this);
00045 }
00046
00047
00048
00049
00050
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
00060
00061
00062
00063
00064
00065 PStatCollector &CollisionInvSphere::
00066 get_volume_pcollector() {
00067 return _volume_pcollector;
00068 }
00069
00070
00071
00072
00073
00074
00075
00076
00077 PStatCollector &CollisionInvSphere::
00078 get_test_pcollector() {
00079 return _test_pcollector;
00080 }
00081
00082
00083
00084
00085
00086
00087 void CollisionInvSphere::
00088 output(ostream &out) const {
00089 out << "invsphere, c (" << get_center() << "), r " << get_radius();
00090 }
00091
00092
00093
00094
00095
00096
00097 PT(BoundingVolume) CollisionInvSphere::
00098 compute_internal_bounds() const {
00099
00100
00101 return new OmniBoundingVolume();
00102 }
00103
00104
00105
00106
00107
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
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
00142
00143
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
00160
00161
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
00176
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
00203
00204
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
00219
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
00249
00250
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
00266
00267 t1 = t2 = 0.0;
00268 }
00269
00270 double t;
00271 if (t2 <= 0.0) {
00272
00273 t = 0.0;
00274
00275 } else if (t1 >= 1.0) {
00276
00277 t = 1.0;
00278
00279 } else if (t2 <= 1.0) {
00280
00281 t = min(t2, 1.0);
00282
00283 } else if (t1 >= 0.0) {
00284
00285 t = max(t1, 0.0);
00286
00287 } else {
00288
00289
00290
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
00317
00318
00319
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
00360
00361
00362
00363 void CollisionInvSphere::
00364 register_with_read_factory() {
00365 BamReader::get_factory()->register_factory(get_class_type(), make_CollisionInvSphere);
00366 }
00367
00368
00369
00370
00371
00372
00373
00374 void CollisionInvSphere::
00375 write_datagram(BamWriter *manager, Datagram &me) {
00376 CollisionSphere::write_datagram(manager, me);
00377 }
00378
00379
00380
00381
00382
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
00397
00398
00399
00400
00401
00402
00403 void CollisionInvSphere::
00404 fillin(DatagramIterator& scan, BamReader* manager) {
00405 CollisionSphere::fillin(scan, manager);
00406 }
00407