From 1aab3341c94644d3aea5073cb0c65c139bcc1c20 Mon Sep 17 00:00:00 2001 From: Dave Schuyler Date: Wed, 3 May 2006 00:58:13 +0000 Subject: [PATCH] ds_solid --- panda/src/collide/Sources.pp | 3 + panda/src/collide/collide_composite1.cxx | 1 + panda/src/collide/collisionSolid.h | 3 + panda/src/collide/collisionSphere.cxx | 123 ++++++++++++++++++++++- panda/src/collide/collisionSphere.h | 2 + panda/src/collide/config_collide.cxx | 3 + 6 files changed, 133 insertions(+), 2 deletions(-) diff --git a/panda/src/collide/Sources.pp b/panda/src/collide/Sources.pp index a63bbe9981..39ea7746d1 100644 --- a/panda/src/collide/Sources.pp +++ b/panda/src/collide/Sources.pp @@ -19,6 +19,7 @@ collisionHandlerPhysical.I collisionHandlerPhysical.h \ collisionHandlerPusher.I collisionHandlerPusher.h \ collisionHandlerQueue.h \ + collisionDSSolid.I collisionDSSolid.h \ collisionInvSphere.I collisionInvSphere.h \ collisionLine.I collisionLine.h \ collisionLevelState.I collisionLevelState.h \ @@ -46,6 +47,7 @@ collisionHandlerPusher.cxx \ collisionHandlerQueue.cxx \ collisionLevelState.cxx \ + collisionDSSolid.cxx \ collisionInvSphere.cxx \ collisionLine.cxx \ collisionNode.cxx \ @@ -71,6 +73,7 @@ collisionHandlerPhysical.I collisionHandlerPhysical.h \ collisionHandlerPusher.I collisionHandlerPusher.h \ collisionHandlerQueue.h \ + collisionDSSolid.I collisionDSSolid.h \ collisionInvSphere.I collisionInvSphere.h \ collisionLevelState.I collisionLevelState.h \ collisionLine.I collisionLine.h \ diff --git a/panda/src/collide/collide_composite1.cxx b/panda/src/collide/collide_composite1.cxx index eca3f27159..6395e76925 100644 --- a/panda/src/collide/collide_composite1.cxx +++ b/panda/src/collide/collide_composite1.cxx @@ -8,6 +8,7 @@ #include "collisionHandlerPhysical.cxx" #include "collisionHandlerPusher.cxx" #include "collisionHandlerQueue.cxx" +#include "collisionDSSolid.cxx" #include "collisionInvSphere.cxx" #include "collisionLevelState.cxx" #include "collisionLine.cxx" diff --git a/panda/src/collide/collisionSolid.h b/panda/src/collide/collisionSolid.h index 1a430b3dec..00aba7c621 100644 --- a/panda/src/collide/collisionSolid.h +++ b/panda/src/collide/collisionSolid.h @@ -95,6 +95,8 @@ protected: INLINE void mark_internal_bounds_stale(); virtual PT(BoundingVolume) compute_internal_bounds() const; + virtual PT(CollisionEntry) + test_intersection_from_ds_solid(const CollisionEntry &entry) const; virtual PT(CollisionEntry) test_intersection_from_sphere(const CollisionEntry &entry) const; virtual PT(CollisionEntry) @@ -164,6 +166,7 @@ public: private: static TypeHandle _type_handle; + friend class CollisionDSSolid; friend class CollisionSphere; friend class CollisionLine; friend class CollisionRay; diff --git a/panda/src/collide/collisionSphere.cxx b/panda/src/collide/collisionSphere.cxx index 125a90fe31..74e68f2223 100644 --- a/panda/src/collide/collisionSphere.cxx +++ b/panda/src/collide/collisionSphere.cxx @@ -17,6 +17,7 @@ //////////////////////////////////////////////////////////////////// +#include "collisionDSSolid.h" #include "collisionSphere.h" #include "collisionLine.h" #include "collisionRay.h" @@ -36,8 +37,10 @@ #include "geomTristrips.h" #include "geomVertexWriter.h" -PStatCollector CollisionSphere::_volume_pcollector("Collision Volumes:CollisionSphere"); -PStatCollector CollisionSphere::_test_pcollector("Collision Tests:CollisionSphere"); +PStatCollector CollisionSphere::_volume_pcollector( + "Collision Volumes:CollisionSphere"); +PStatCollector CollisionSphere::_test_pcollector( + "Collision Tests:CollisionSphere"); TypeHandle CollisionSphere::_type_handle; //////////////////////////////////////////////////////////////////// @@ -133,6 +136,122 @@ PT(BoundingVolume) CollisionSphere:: compute_internal_bounds() const { return new BoundingSphere(_center, _radius); } +#define USE_DS_SOLID_PLANES 0 +//////////////////////////////////////////////////////////////////// +// Function: CollisionSphere::test_intersection_from_ds_solid +// Access: Public, Virtual +// Description: +//////////////////////////////////////////////////////////////////// +PT(CollisionEntry) CollisionSphere:: +test_intersection_from_ds_solid(const CollisionEntry &entry) const { + const CollisionDSSolid *ds_solid; + DCAST_INTO_R(ds_solid, entry.get_from(), 0); + cerr<<"CollisionSphere::test_intersection_from_ds_solid\n"; + + CPT(TransformState) wrt_space = entry.get_wrt_space(); + + const LMatrix4f &wrt_mat = wrt_space->get_mat(); + + LPoint3f into_center = get_center(); + float into_radius = get_radius(); + + LPoint3f sa_center = ds_solid->get_center_a() * wrt_mat; + float sa_radius = length( + LVector3f(ds_solid->get_radius_a(), 0.0f, 0.0f) * wrt_mat); + LVector3f sa_vec = sa_center - into_center; + float sa_distance_squared = dot(sa_vec, sa_vec); + float sa_and_into_radii_squared = ( + sa_radius + into_radius) * (sa_radius + into_radius); + if (sa_distance_squared > sa_and_into_radii_squared) { + // No intersection. + return NULL; + } + + LPoint3f sb_center = ds_solid->get_center_b() * wrt_mat; + float sb_radius = length( + LVector3f(ds_solid->get_radius_b(), 0.0f, 0.0f) * wrt_mat); + LVector3f sb_vec = sb_center - into_center; + float sb_distance_squared = dot(sb_vec, sb_vec); + float sb_and_into_radii_squared = ( + sb_radius + into_radius) * (sb_radius + into_radius); + if (sb_distance_squared > sb_and_into_radii_squared) { + // No intersection. + return NULL; + } + + #if USE_DS_SOLID_PLANES + float pa_distance = ds_solid->dist_to_plane_a(into_center); + if (pa_distance > into_radius) { + // No intersection. + return NULL; + } + + float pb_distance = ds_solid->dist_to_plane_b(into_center); + if (pb_distance > into_radius) { + // No intersection. + return NULL; + } + #endif + + if (collide_cat.is_debug()) { + collide_cat.debug() + << "intersection detected from " << entry.get_from_node_path() + << " into " << entry.get_into_node_path() << "\n"; + } + + LVector3f surface_normal; // into + LPoint3f surface_point; // into + LPoint3f interior_point; // from + float spheres = sqrtf(sa_distance_squared) - sqrtf(sb_distance_squared); + #if USE_DS_SOLID_PLANES + float planes = pa_distance - pb_distance; + if (spheres > planes) { + #endif + if (spheres > 0) { + // sphere_a is the furthest + cerr<<"sphere_a is the furthest"<<"\n"; + float vec_length = sa_vec.length(); + if (IS_NEARLY_ZERO(vec_length)) { + // The centers are coincident, use an arbitrary normal. + surface_normal.set(1.0, 0.0, 0.0); + } else { + surface_normal = sa_vec / vec_length; + } + interior_point = sa_center - surface_normal * sa_radius; + } else { + // sphere_b is the furthest + cerr<<"sphere_b is the furthest"<<"\n"; + float vec_length = sb_vec.length(); + if (IS_NEARLY_ZERO(vec_length)) { + // The centers are coincident, use an arbitrary normal. + surface_normal.set(1.0, 0.0, 0.0); + } else { + surface_normal = sb_vec / vec_length; + } + interior_point = sb_center - surface_normal * sb_radius; + } + #if USE_DS_SOLID_PLANES + } else { + if (planes > 0) { + // plane_a is the furthest + cerr<<"plane_a is the furthest"<<"\n"; + surface_normal = ds_solid->get_plane_a().get_normal(); + interior_point = into_center - surface_normal * pa_distance; + } else { + // plane_b is the furthest + cerr<<"plane_b is the furthest"<<"\n"; + surface_normal = ds_solid->get_plane_b().get_normal(); + interior_point = into_center - surface_normal * pb_distance; + } + } + #endif + + PT(CollisionEntry) new_entry = new CollisionEntry(entry); + new_entry->set_surface_normal(surface_normal); + new_entry->set_surface_point(into_center + surface_normal * into_radius); + new_entry->set_interior_point(interior_point); + return new_entry; +} //////////////////////////////////////////////////////////////////// // Function: CollisionSphere::test_intersection_from_sphere diff --git a/panda/src/collide/collisionSphere.h b/panda/src/collide/collisionSphere.h index add1018371..7b9a025c4c 100644 --- a/panda/src/collide/collisionSphere.h +++ b/panda/src/collide/collisionSphere.h @@ -63,6 +63,8 @@ PUBLISHED: protected: virtual PT(BoundingVolume) compute_internal_bounds() const; + virtual PT(CollisionEntry) + test_intersection_from_ds_solid(const CollisionEntry &entry) const; virtual PT(CollisionEntry) test_intersection_from_sphere(const CollisionEntry &entry) const; virtual PT(CollisionEntry) diff --git a/panda/src/collide/config_collide.cxx b/panda/src/collide/config_collide.cxx index 7eeb3de03b..8f0b5482d1 100644 --- a/panda/src/collide/config_collide.cxx +++ b/panda/src/collide/config_collide.cxx @@ -25,6 +25,7 @@ #include "collisionHandlerPhysical.h" #include "collisionHandlerPusher.h" #include "collisionHandlerQueue.h" +#include "collisionDSSolid.h" #include "collisionInvSphere.h" #include "collisionLine.h" #include "collisionNode.h" @@ -87,6 +88,7 @@ init_libcollide() { CollisionHandlerPhysical::init_type(); CollisionHandlerPusher::init_type(); CollisionHandlerQueue::init_type(); + CollisionDSSolid::init_type(); CollisionInvSphere::init_type(); CollisionLine::init_type(); CollisionNode::init_type(); @@ -103,6 +105,7 @@ init_libcollide() { CollisionVisualizer::init_type(); #endif + CollisionDSSolid::register_with_read_factory(); CollisionInvSphere::register_with_read_factory(); CollisionLine::register_with_read_factory(); CollisionNode::register_with_read_factory();