ds_solid

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 \

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"

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;

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

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)

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();