00001
00002
00003
00004
00005
00006
00007
00008
00009
00010
00011
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
00043
00044
00045
00046 CollisionSolid *CollisionPlane::
00047 make_copy() {
00048 return new CollisionPlane(*this);
00049 }
00050
00051
00052
00053
00054
00055
00056 void CollisionPlane::
00057 xform(const LMatrix4 &mat) {
00058 _plane = _plane * mat;
00059 CollisionSolid::xform(mat);
00060 }
00061
00062
00063
00064
00065
00066
00067
00068
00069
00070 LPoint3 CollisionPlane::
00071 get_collision_origin() const {
00072
00073
00074
00075 return LPoint3::origin();
00076 }
00077
00078
00079
00080
00081
00082
00083
00084
00085 PStatCollector &CollisionPlane::
00086 get_volume_pcollector() {
00087 return _volume_pcollector;
00088 }
00089
00090
00091
00092
00093
00094
00095
00096
00097 PStatCollector &CollisionPlane::
00098 get_test_pcollector() {
00099 return _test_pcollector;
00100 }
00101
00102
00103
00104
00105
00106
00107 void CollisionPlane::
00108 output(ostream &out) const {
00109 out << "cplane, (" << _plane << ")";
00110 }
00111
00112
00113
00114
00115
00116
00117 PT(BoundingVolume) CollisionPlane::
00118 compute_internal_bounds() const {
00119 return new BoundingPlane(_plane);
00120 }
00121
00122
00123
00124
00125
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
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
00165
00166
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
00181
00182 if (_plane.dist_to_plane(from_origin) > 0.0f) {
00183
00184 return NULL;
00185 }
00186
00187
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
00212
00213
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
00229
00230 t = 0.0f;
00231
00232 } else {
00233 if (!_plane.intersects_line(t, from_origin, from_direction)) {
00234
00235 return NULL;
00236 }
00237
00238 if (t < 0.0f) {
00239
00240
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
00266
00267
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
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
00301 if (t >= 0.0f && t <= 1.0f) {
00302
00303 new_entry->set_surface_point(from_a + t * from_direction);
00304 }
00305 }
00306
00307 if (dist_a < dist_b) {
00308
00309 new_entry->set_interior_point(from_a);
00310
00311 } else if (dist_b < dist_a) {
00312
00313 new_entry->set_interior_point(from_b);
00314
00315 } else {
00316
00317 new_entry->set_interior_point((from_a + from_b) * 0.5);
00318 }
00319
00320 return new_entry;
00321 }
00322
00323
00324
00325
00326
00327
00328
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
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
00344
00345 t = parabola->get_t1();
00346
00347 } else {
00348 PN_stdfloat t1, t2;
00349 if (!get_plane().intersects_parabola(t1, t2, local_p)) {
00350
00351
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
00358
00359 t = min(t1, t2);
00360 } else {
00361
00362 t = t1;
00363 }
00364
00365 } else if (t2 >= parabola->get_t1() && t2 <= parabola->get_t2()) {
00366
00367 t = t2;
00368
00369 } else {
00370
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
00394
00395
00396
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
00406
00407
00408
00409
00410
00411
00412
00413
00414
00415
00416
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
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
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
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
00482
00483
00484
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
00495
00496
00497
00498
00499
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
00510
00511
00512
00513 TypedWritable* CollisionPlane::
00514 make_CollisionPlane(const FactoryParams ¶ms)
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
00527
00528
00529
00530 void CollisionPlane::
00531 register_with_read_factory()
00532 {
00533 BamReader::get_factory()->register_factory(get_class_type(), make_CollisionPlane);
00534 }