diff --git a/panda/src/collide/collisionDSSolid.I b/panda/src/collide/collisionDSSolid.I new file mode 100755 index 0000000000..c193ee57a7 --- /dev/null +++ b/panda/src/collide/collisionDSSolid.I @@ -0,0 +1,286 @@ +// Filename: collisionDSSolid.I +// Created by: Dave Schuyler (05Apr06) +// Based on collision tube by: drose +// +//////////////////////////////////////////////////////////////////// +// +// PANDA 3D SOFTWARE +// Copyright (c) 2001 - 2004, Disney Enterprises, Inc. All rights reserved +// +// All use of this software is subject to the terms of the Panda 3d +// Software license. You should have received a copy of this license +// along with this source code; you will also find a current copy of +// the license at http://etc.cmu.edu/panda3d/docs/license/ . +// +// To contact the maintainers of this program write to +// panda3d-general@lists.sourceforge.net . +// +//////////////////////////////////////////////////////////////////// + + +//////////////////////////////////////////////////////////////////// +// Function: Collision::Constructor +// Access: Public +// Description: +//////////////////////////////////////////////////////////////////// +INLINE CollisionDSSolid:: +CollisionDSSolid( + const LPoint3f ¢er_a, float radius_a, + const LPoint3f ¢er_b, float radius_b, + const Planef &plane_a, const Planef &plane_b) : + _center_a(center_a), _radius_a(radius_a), + _center_b(center_b), _radius_b(radius_b), + _plane_a(plane_a), _plane_b(plane_b) +{ + recalc_internals(); + nassertv(_radius_a >= 0.0f); + nassertv(_radius_b >= 0.0f); +} + +//////////////////////////////////////////////////////////////////// +// Function: CollisionDSSolid::Constructor +// Access: Public +// Description: +//////////////////////////////////////////////////////////////////// +INLINE CollisionDSSolid:: +CollisionDSSolid( + float ax, float ay, float az, + float radius_a, + float bx, float by, float bz, + float radius_b, + const Planef &plane_a, const Planef &plane_b) : + _center_a(ax, ay, az), _radius_a(radius_a), + _center_b(bx, by, bz), _radius_b(radius_b), + _plane_a(plane_a), _plane_b(plane_b) +{ + recalc_internals(); + nassertv(_radius_a >= 0.0f); + nassertv(_radius_b >= 0.0f); +} + +//////////////////////////////////////////////////////////////////// +// Function: CollisionDSSolid::Default constructor +// Access: Private +// Description: Creates an invalid tube. Only used when reading +// from a bam file. +//////////////////////////////////////////////////////////////////// +INLINE CollisionDSSolid:: +CollisionDSSolid() { +} + +//////////////////////////////////////////////////////////////////// +// Function: CollisionDSSolid::Copy Constructor +// Access: Public +// Description: +//////////////////////////////////////////////////////////////////// +INLINE CollisionDSSolid:: +CollisionDSSolid(const CollisionDSSolid ©) : + CollisionSolid(copy), + _center_a(copy._center_a), + _radius_a(copy._radius_a), + _center_b(copy._center_b), + _radius_b(copy._radius_b), + _plane_a(copy._plane_a), + _plane_b(copy._plane_b) +{ + recalc_internals(); +} + +//////////////////////////////////////////////////////////////////// +// Function: CollisionDSSolid::set_center_a +// Access: Public +// Description: +//////////////////////////////////////////////////////////////////// +INLINE void CollisionDSSolid:: +set_center_a(const LPoint3f &a) { + _center_a = a; + recalc_internals(); +} + +//////////////////////////////////////////////////////////////////// +// Function: CollisionDSSolid::set_center_a +// Access: Public +// Description: +//////////////////////////////////////////////////////////////////// +INLINE void CollisionDSSolid:: +set_center_a(float x, float y, float z) { + set_center_a(LPoint3f(x, y, z)); +} + +//////////////////////////////////////////////////////////////////// +// Function: CollisionDSSolid::get_center_a +// Access: Public +// Description: +//////////////////////////////////////////////////////////////////// +INLINE const LPoint3f &CollisionDSSolid:: +get_center_a() const { + return _center_a; +} + +//////////////////////////////////////////////////////////////////// +// Function: CollisionDSSolid::set_center_b +// Access: Public +// Description: +//////////////////////////////////////////////////////////////////// +INLINE void CollisionDSSolid:: +set_center_b(const LPoint3f &b) { + _center_b = b; + recalc_internals(); +} + +//////////////////////////////////////////////////////////////////// +// Function: CollisionDSSolid::set_center_b +// Access: Public +// Description: +//////////////////////////////////////////////////////////////////// +INLINE void CollisionDSSolid:: +set_center_b(float x, float y, float z) { + set_center_b(LPoint3f(x, y, z)); +} + +//////////////////////////////////////////////////////////////////// +// Function: CollisionDSSolid::get_center_b +// Access: Public +// Description: +//////////////////////////////////////////////////////////////////// +INLINE const LPoint3f &CollisionDSSolid:: +get_center_b() const { + return _center_b; +} + +//////////////////////////////////////////////////////////////////// +// Function: CollisionDSSolid::set_radius_a +// Access: Public +// Description: +//////////////////////////////////////////////////////////////////// +INLINE void CollisionDSSolid:: +set_radius_a(float radius) { + nassertv(radius >= 0.0f); + _radius_a = radius; + + // We don't need to call recalc_internals(), since the radius + // doesn't change either of those properties. + mark_internal_bounds_stale(); + mark_viz_stale(); +} + +//////////////////////////////////////////////////////////////////// +// Function: CollisionDSSolid::get_radius_a +// Access: Public +// Description: +//////////////////////////////////////////////////////////////////// +INLINE float CollisionDSSolid:: +get_radius_a() const { + return _radius_a; +} + +//////////////////////////////////////////////////////////////////// +// Function: CollisionDSSolid::set_radius_b +// Access: Public +// Description: +//////////////////////////////////////////////////////////////////// +INLINE void CollisionDSSolid:: +set_radius_b(float radius) { + nassertv(radius >= 0.0f); + _radius_b = radius; + + // We don't need to call recalc_internals(), since the radius + // doesn't change either of those properties. + mark_internal_bounds_stale(); + mark_viz_stale(); +} + +//////////////////////////////////////////////////////////////////// +// Function: CollisionDSSolid::get_radius_b +// Access: Public +// Description: +//////////////////////////////////////////////////////////////////// +INLINE float CollisionDSSolid:: +get_radius_b() const { + return _radius_b; +} + +//////////////////////////////////////////////////////////////////// +// Function: CollisionDSSolid::get_normal_a +// Access: Published +// Description: +//////////////////////////////////////////////////////////////////// +INLINE LVector3f CollisionDSSolid:: +get_normal_a() const { + return _plane_a.get_normal(); +} + +//////////////////////////////////////////////////////////////////// +// Function: CollisionDSSolid::dist_to_plane_a +// Access: Published +// Description: +//////////////////////////////////////////////////////////////////// +INLINE float CollisionDSSolid:: +dist_to_plane_a(const LPoint3f &point) const { + return _plane_a.dist_to_plane(point); +} + +//////////////////////////////////////////////////////////////////// +// Function: CollisionDSSolid::set_plane_a +// Access: Published +// Description: +//////////////////////////////////////////////////////////////////// +INLINE void CollisionDSSolid:: +set_plane_a(const Planef &plane) { + _plane_a = plane; + mark_internal_bounds_stale(); + mark_viz_stale(); +} + +//////////////////////////////////////////////////////////////////// +// Function: CollisionDSSolid::get_plane_a +// Access: Published +// Description: +//////////////////////////////////////////////////////////////////// +INLINE const Planef &CollisionDSSolid:: +get_plane_a() const { + return _plane_a; +} + +//////////////////////////////////////////////////////////////////// +// Function: CollisionDSSolid::get_normal_b +// Access: Published +// Description: +//////////////////////////////////////////////////////////////////// +INLINE LVector3f CollisionDSSolid:: +get_normal_b() const { + return _plane_b.get_normal(); +} + +//////////////////////////////////////////////////////////////////// +// Function: CollisionDSSolid::dist_to_plane_b +// Access: Published +// Description: +//////////////////////////////////////////////////////////////////// +INLINE float CollisionDSSolid:: +dist_to_plane_b(const LPoint3f &point) const { + return _plane_b.dist_to_plane(point); +} + +//////////////////////////////////////////////////////////////////// +// Function: CollisionDSSolid::set_plane_b +// Access: Published +// Description: +//////////////////////////////////////////////////////////////////// +INLINE void CollisionDSSolid:: +set_plane_b(const Planef &plane) { + _plane_b = plane; + mark_internal_bounds_stale(); + mark_viz_stale(); +} + +//////////////////////////////////////////////////////////////////// +// Function: CollisionDSSolid::get_plane_b +// Access: Published +// Description: +//////////////////////////////////////////////////////////////////// +INLINE const Planef &CollisionDSSolid:: +get_plane_b() const { + return _plane_b; +} + diff --git a/panda/src/collide/collisionDSSolid.cxx b/panda/src/collide/collisionDSSolid.cxx new file mode 100755 index 0000000000..fe1fddab9e --- /dev/null +++ b/panda/src/collide/collisionDSSolid.cxx @@ -0,0 +1,561 @@ +// Filename: collisionDSSolid.cxx +// Created by: Dave Schuyler (05Apr06) +// Based on collision tube by: drose +// +//////////////////////////////////////////////////////////////////// +// +// PANDA 3D SOFTWARE +// Copyright (c) 2001 - 2004, Disney Enterprises, Inc. All rights reserved +// +// All use of this software is subject to the terms of the Panda 3d +// Software license. You should have received a copy of this license +// along with this source code; you will also find a current copy of +// the license at http://etc.cmu.edu/panda3d/docs/license/ . +// +// To contact the maintainers of this program write to +// panda3d-general@lists.sourceforge.net . +// +//////////////////////////////////////////////////////////////////// + +#include "collisionDSSolid.h" +#include "collisionSphere.h" +#include "collisionLine.h" +#include "collisionRay.h" +#include "collisionSegment.h" +#include "collisionHandler.h" +#include "collisionEntry.h" +#include "boundingSphere.h" +#include "config_collide.h" +#include "look_at.h" +#include "geom.h" +#include "geomNode.h" +#include "geometricBoundingVolume.h" +#include "datagram.h" +#include "datagramIterator.h" +#include "bamReader.h" +#include "bamWriter.h" +#include "cmath.h" +#include "transformState.h" +#include "geom.h" +#include "geomTristrips.h" +#include "geomVertexWriter.h" + +PStatCollector CollisionDSSolid::_volume_pcollector( + "Collision Volumes:CollisionDSSolid"); +PStatCollector CollisionDSSolid::_test_pcollector( + "Collision Tests:CollisionDSSolid"); +TypeHandle CollisionDSSolid::_type_handle; + +//////////////////////////////////////////////////////////////////// +// Function: CollisionDSSolid::make_copy +// Access: Public, Virtual +// Description: +//////////////////////////////////////////////////////////////////// +CollisionSolid *CollisionDSSolid:: +make_copy() { + return new CollisionDSSolid(*this); +} + +//////////////////////////////////////////////////////////////////// +// Function: CollisionDSSolid::xform +// Access: Public, Virtual +// Description: Transforms the solid by the indicated matrix. +//////////////////////////////////////////////////////////////////// +void CollisionDSSolid:: +xform(const LMatrix4f &mat) { + _center_a = _center_a * mat; + _center_b = _center_b * mat; + _plane_a = _plane_a * mat; + _plane_b = _plane_b * mat; + + // This is a little cheesy and fails miserably in the presence of a + // non-uniform scale. + LVector3f radius_v = LVector3f(_radius_a, 0.0f, 0.0f) * mat; + _radius_a = length(radius_v); + + recalc_internals(); + CollisionSolid::xform(mat); +} + +//////////////////////////////////////////////////////////////////// +// Function: CollisionDSSolid::get_collision_origin +// Access: Public, Virtual +// Description: Returns the point in space deemed to be the "origin" +// of the solid for collision purposes. The closest +// intersection point to this origin point is considered +// to be the most significant. +//////////////////////////////////////////////////////////////////// +LPoint3f CollisionDSSolid:: +get_collision_origin() const { + #if 1 + LVector3f vec = _center_b - _center_a; + float distance = vec.length(); + if (vec.normalize()) { + // "Let" a few variables + float dist_squared = distance * distance; + float dist_doubled = 2.0f * distance; + float radius_a_squared = _radius_a * _radius_a; + float radius_b_squared = _radius_b * _radius_b; + float n = dist_squared - radius_a_squared + radius_b_squared; + + // Get the lens center point on the intersection plane between + // sphere A and sphere B + LVector3f lens_center = _center_a + vec * (n / dist_doubled); + return lens_center; + } else { + // Since both spheres are in the same place, just return + // either of the centers + return _center_a; + } + #else + LVector3f vec = _center_b - _center_a; + return _center_a + (vec * 0.5); + #endif +} + +//////////////////////////////////////////////////////////////////// +// Function: CollisionDSSolid::get_volume_pcollector +// Access: Public, Virtual +// Description: Returns a PStatCollector that is used to count the +// number of bounding volume tests made against a solid +// of this type in a given frame. +//////////////////////////////////////////////////////////////////// +PStatCollector CollisionDSSolid:: +get_volume_pcollector() { + return _volume_pcollector; +} + +//////////////////////////////////////////////////////////////////// +// Function: CollisionDSSolid::get_test_pcollector +// Access: Public, Virtual +// Description: Returns a PStatCollector that is used to count the +// number of intersection tests made against a solid +// of this type in a given frame. +//////////////////////////////////////////////////////////////////// +PStatCollector CollisionDSSolid:: +get_test_pcollector() { + return _test_pcollector; +} + +//////////////////////////////////////////////////////////////////// +// Function: CollisionDSSolid::output +// Access: Public, Virtual +// Description: +//////////////////////////////////////////////////////////////////// +void CollisionDSSolid:: +output(ostream &out) const { + out << "DSSolid, a (" + << _center_a << "), ra " + << _radius_a << ", b (" + << _center_b << "), rb " + << _radius_b << ", pa (" + << _plane_a << "), pb " + << _plane_b << ")"; +} + +//////////////////////////////////////////////////////////////////// +// Function: CollisionDSSolid::compute_internal_bounds +// Access: Protected, Virtual +// Description: +//////////////////////////////////////////////////////////////////// +PT(BoundingVolume) CollisionDSSolid:: +compute_internal_bounds() const { + PT(BoundingVolume) bound = CollisionSolid::compute_internal_bounds(); + + if (bound->is_of_type(GeometricBoundingVolume::get_class_type())) { + GeometricBoundingVolume *gbound; + DCAST_INTO_R(gbound, bound, bound); + + LVector3f vec = _center_b - _center_a; + float distance = vec.length(); + if (vec.normalize()) { + // There is some distance between the centers, so the intersection + // between them is some kind of lens. + + // "Let" a few variables + float dist_squared = distance * distance; + float dist_doubled = 2.0f * distance; + float radius_a_squared = _radius_a * _radius_a; + float radius_b_squared = _radius_b * _radius_b; + float n = dist_squared - radius_a_squared + radius_b_squared; + float m = 4.0f * dist_squared * radius_a_squared - (n * n); + + cerr<<"distance:"< 0.0f); + + // Get the lens center point on the intersection plane between + // sphere A and sphere B + LVector3f lens_center = _center_a + vec * (n / dist_doubled); + float lens_radius = (1.0f / dist_doubled) * sqrt(m); + cerr<<"lens_center:"<extend_by(&sphere); + } else { + // Both endpoints are coincident; therefore, the bounding volume + // is a sphere. + //TODO: account for cutting planes (which could make the sphere + // smaller, which is an optimization). + BoundingSphere sphere(_center_a, _radius_a); + gbound->extend_by(&sphere); + } + } + + return bound; +} + +//////////////////////////////////////////////////////////////////// +// Function: CollisionDSSolid::test_intersection_from_sphere +// Access: Public, Virtual +// Description: +//////////////////////////////////////////////////////////////////// +PT(CollisionEntry) CollisionDSSolid:: +test_intersection_from_sphere(const CollisionEntry &entry) const { + const CollisionSphere *sphere; + DCAST_INTO_R(sphere, entry.get_from(), 0); + + CPT(TransformState) wrt_space = entry.get_wrt_space(); + + const LMatrix4f &wrt_mat = wrt_space->get_mat(); + + LPoint3f from_a = sphere->get_center() * wrt_mat; + LPoint3f from_b = from_a; + + LVector3f from_direction = from_b - from_a; + + LPoint3f from_center = sphere->get_center() * wrt_mat; + LVector3f from_radius_v = + LVector3f(sphere->get_radius(), 0.0f, 0.0f) * wrt_mat; + float from_radius = length(from_radius_v); + + LPoint3f sa_into_center = get_center_a(); + float sa_into_radius = get_radius_a(); + LVector3f sa_vec = from_center - sa_into_center; + float sa_distance_squared = dot(sa_vec, sa_vec); + float sa_and_from_radii_squared = ( + sa_into_radius + from_radius) * (sa_into_radius + from_radius); + if (sa_distance_squared > sa_and_from_radii_squared) { + // No intersection. + return NULL; + } + + LPoint3f sb_into_center = get_center_b(); + float sb_into_radius = get_radius_b(); + LVector3f sb_vec = from_center - sb_into_center; + float sb_distance_squared = dot(sb_vec, sb_vec); + float sb_and_from_radii_squared = ( + sb_into_radius + from_radius) * (sb_into_radius + from_radius); + if (sb_distance_squared > sb_and_from_radii_squared) { + // No intersection. + return NULL; + } + + float pa_distance = dist_to_plane_a(from_center); + if (pa_distance > from_radius) { + // No intersection. + return NULL; + } + + float pb_distance = dist_to_plane_b(from_center); + if (pb_distance > from_radius) { + // No intersection. + return NULL; + } + + 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; + LPoint3f surface_point; + float spheres = sa_distance_squared - sb_distance_squared; + float planes = pa_distance * pa_distance - pb_distance * pb_distance; + if (spheres > planes) { + 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; + } + surface_point = sa_into_center + surface_normal * sa_into_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; + } + surface_point = sb_into_center + surface_normal * sb_into_radius; + } + } else { + if (planes > 0) { + // plane_a is the furthest + cerr<<"plane_a is the furthest"<<"\n"; + surface_normal = _plane_a.get_normal(); + surface_point = from_center - surface_normal * pa_distance; + } else { + // plane_b is the furthest + cerr<<"plane_b is the furthest"<<"\n"; + surface_normal = _plane_b.get_normal(); + surface_point = from_center - surface_normal * pb_distance; + } + } + + PT(CollisionEntry) new_entry = new CollisionEntry(entry); + new_entry->set_surface_normal(surface_normal); + new_entry->set_surface_point(surface_point); + new_entry->set_interior_point(from_center - surface_normal * from_radius); + return new_entry; +} + +//////////////////////////////////////////////////////////////////// +// Function: CollisionDSSolid::fill_viz_geom +// Access: Protected, Virtual +// Description: Fills the _viz_geom GeomNode up with Geoms suitable +// for rendering this solid. +//////////////////////////////////////////////////////////////////// +void CollisionDSSolid:: +fill_viz_geom() { + if (collide_cat.is_debug()) { + collide_cat.debug() + << "Recomputing viz for " << *this << "\n"; + } + + // Generate the vertices such that we draw a shape with one sphere + // at (0, 0, 0), and another at (0, length, 0). Then we'll rotate + // and translate it into place with the appropriate look_at matrix. + LVector3f direction = _center_b - _center_a; + float half_length = direction.length() * 0.5f; + + // "Let" a few variables + float distance = direction.length(); + float dist_squared = distance * distance; + float dist_doubled = 2.0f * distance; + float radius_a_squared = _radius_a * _radius_a; + float radius_b_squared = _radius_b * _radius_b; + float triangle_height = ( + dist_squared - radius_a_squared + radius_b_squared) / dist_doubled; + + // Get (half) the angle from _center_a to the lens edge on the + // intersection plane between sphere A and sphere B + float half_arc_angle_a = acos(triangle_height / _radius_a); + float half_arc_angle_b = acos(triangle_height / _radius_b); + + PT(GeomVertexData) vdata = new GeomVertexData( + "collision", GeomVertexFormat::get_v3cp(), Geom::UH_static); + GeomVertexWriter vertex(vdata, InternalName::get_vertex()); + GeomVertexWriter color(vdata, InternalName::get_color()); + + PT(GeomTristrips) strip = new GeomTristrips(Geom::UH_static); + // Generate the first endcap. + static const int num_slices = 8; + static const int num_rings = 4; + int ri, si; + for (ri = 0; ri < num_rings; ++ri) { + for (si = 0; si <= num_slices; ++si) { + vertex.add_data3f(calc_sphere2_vertex( + ri, si, num_rings, num_slices, -half_length, half_arc_angle_a)); + //color.add_data4f(Colorf(1.0f, 0.0f, 0.0f, 1.0f)); + color.add_data4f(1.0f, 0.0f, 0.0f, 1.0f); + vertex.add_data3f(calc_sphere2_vertex( + ri + 1, si, num_rings, num_slices, -half_length, half_arc_angle_a)); + //color.add_data4f(Colorf(1.0f, 0.0f, 0.0f, 1.0f)); + color.add_data4f(1.0f, 0.0f, 0.0f, 1.0f); + } + strip->add_next_vertices((num_slices + 1) * 2); + strip->close_primitive(); + } + + #if 0 + // Now the planes. + for (si = 0; si <= num_slices; ++si) { + vertex.add_data3f(calc_sphere1_vertex( + num_rings, si, num_rings, num_slices)); + vertex.add_data3f(calc_sphere2_vertex( + num_rings, si, num_rings, num_slices, half_length)); + } + strip->add_next_vertices((num_slices + 1) * 2); + strip->close_primitive(); + #endif + + // And the second endcap. + for (ri = num_rings - 1; ri >= 0; --ri) { + for (si = 0; si <= num_slices; ++si) { + vertex.add_data3f(calc_sphere1_vertex( + ri + 1, si, num_rings, num_slices, half_length, half_arc_angle_b)); + color.add_data4f(Colorf(0.0f, 0.0f, 1.0f, 1.0f)); + vertex.add_data3f(calc_sphere1_vertex( + ri, si, num_rings, num_slices, half_length, half_arc_angle_b)); + color.add_data4f(Colorf(1.0f, 1.0f, 1.0f, 1.0f)); + } + strip->add_next_vertices((num_slices + 1) * 2); + strip->close_primitive(); + } + + PT(Geom) geom = new Geom(vdata); + geom->add_primitive(strip); + + #if 0 + // Now transform the vertices to their actual location. + LMatrix4f mat; + look_at(mat, direction, LVector3f(0.0f, 0.0f, 1.0f), CS_zup_right); + mat.set_row(3, _ceneter_a); + geom->transform_vertices(mat); + #endif + + _viz_geom->add_geom(geom, get_solid_viz_state()); + _bounds_viz_geom->add_geom(geom, get_solid_bounds_viz_state()); +} + +//////////////////////////////////////////////////////////////////// +// Function: CollisionDSSolid::calc_sphere1_vertex +// Access: Private +// Description: Calculates a particular vertex on the surface of the +// first endcap hemisphere, for use in generating the +// viz geometry. +//////////////////////////////////////////////////////////////////// +Vertexf CollisionDSSolid:: +calc_sphere1_vertex(int ri, int si, int num_rings, int num_slices, + float length, float angle) { + float r = (float)ri / (float)num_rings; + float s = (float)si / (float)num_slices; + + // Find the point on the rim, based on the slice. + float theta = s * 2.0f * MathNumbers::pi_f; + float y_rim = ccos(theta); + float z_rim = csin(theta); + + // Now pull that point in towards the pole, based on the ring. + float phi = r * angle; + float to_pole = csin(phi); + + float x = length -_radius_a * ccos(phi); + float y = _radius_a * y_rim * to_pole; + float z = _radius_a * z_rim * to_pole; + + return Vertexf(x, y, z); +} + +//////////////////////////////////////////////////////////////////// +// Function: CollisionDSSolid::calc_sphere2_vertex +// Access: Private +// Description: Calculates a particular vertex on the surface of the +// second endcap hemisphere, for use in generating the +// viz geometry. +//////////////////////////////////////////////////////////////////// +Vertexf CollisionDSSolid:: +calc_sphere2_vertex(int ri, int si, int num_rings, int num_slices, + float length, float angle) { + float r = (float)ri / (float)num_rings; + float s = (float)si / (float)num_slices; + + // Find the point on the rim, based on the slice. + float theta = s * 2.0f * MathNumbers::pi_f; + float y_rim = ccos(theta); + float z_rim = csin(theta); + + // Now pull that point in towards the pole, based on the ring. + float phi = r * angle; + float to_pole = csin(phi); + + float x = length + _radius_b * ccos(phi); + float y = _radius_b * y_rim * to_pole; + float z = _radius_b * z_rim * to_pole; + + return Vertexf(x, y, z); +} + +//////////////////////////////////////////////////////////////////// +// Function: CollisionDSSolid::recalc_internals +// Access: Private +// Description: Should be called internally to recompute the matrix +// and length when the properties of the tube have +// changed. +//////////////////////////////////////////////////////////////////// +void CollisionDSSolid:: +recalc_internals() { + mark_viz_stale(); + mark_internal_bounds_stale(); +} + +//////////////////////////////////////////////////////////////////// +// Function: CollisionDSSolid::register_with_read_factory +// Access: Public, Static +// Description: Tells the BamReader how to create objects of type +// CollisionDSSolid. +//////////////////////////////////////////////////////////////////// +void CollisionDSSolid:: +register_with_read_factory() { + BamReader::get_factory()->register_factory(get_class_type(), make_from_bam); +} + +//////////////////////////////////////////////////////////////////// +// Function: CollisionDSSolid::write_datagram +// Access: Public, Virtual +// Description: Writes the contents of this object to the datagram +// for shipping out to a Bam file. +//////////////////////////////////////////////////////////////////// +void CollisionDSSolid:: +write_datagram(BamWriter *manager, Datagram &dg) { + CollisionSolid::write_datagram(manager, dg); + _center_a.write_datagram(dg); + dg.add_float32(_radius_a); + _center_b.write_datagram(dg); + dg.add_float32(_radius_b); + _plane_a.write_datagram(dg); + _plane_b.write_datagram(dg); +} + +//////////////////////////////////////////////////////////////////// +// Function: CollisionDSSolid::make_from_bam +// Access: Protected, Static +// Description: This function is called by the BamReader's factory +// when a new object of type CollisionDSSolid is encountered +// in the Bam file. It should create the CollisionDSSolid +// and extract its information from the file. +//////////////////////////////////////////////////////////////////// +TypedWritable *CollisionDSSolid:: +make_from_bam(const FactoryParams ¶ms) { + CollisionDSSolid *node = new CollisionDSSolid(); + DatagramIterator scan; + BamReader *manager; + + parse_params(params, scan, manager); + node->fillin(scan, manager); + + return node; +} + +//////////////////////////////////////////////////////////////////// +// Function: CollisionDSSolid::fillin +// Access: Protected +// Description: This internal function is called by make_from_bam to +// read in all of the relevant data from the BamFile for +// the new CollisionDSSolid. +//////////////////////////////////////////////////////////////////// +void CollisionDSSolid:: +fillin(DatagramIterator &scan, BamReader *manager) { + CollisionSolid::fillin(scan, manager); + _center_a.read_datagram(scan); + _radius_a = scan.get_float32(); + _center_b.read_datagram(scan); + _radius_b = scan.get_float32(); + _plane_a.read_datagram(scan); + _plane_b.read_datagram(scan); + recalc_internals(); +} diff --git a/panda/src/collide/collisionDSSolid.h b/panda/src/collide/collisionDSSolid.h new file mode 100755 index 0000000000..e12e94d4c2 --- /dev/null +++ b/panda/src/collide/collisionDSSolid.h @@ -0,0 +1,149 @@ +// Filename: CollisionDSSolid.h +// Created by: Dave Schuyler (05Apr06) +// Based on collision tube by: drose +// +//////////////////////////////////////////////////////////////////// +// +// PANDA 3D SOFTWARE +// Copyright (c) 2001 - 2004, Disney Enterprises, Inc. All rights reserved +// +// All use of this software is subject to the terms of the Panda 3d +// Software license. You should have received a copy of this license +// along with this source code; you will also find a current copy of +// the license at http://etc.cmu.edu/panda3d/docs/license/ . +// +// To contact the maintainers of this program write to +// panda3d-general@lists.sourceforge.net . +// +//////////////////////////////////////////////////////////////////// + +#ifndef COLLISIONDSSOLID_H +#define COLLISIONDSSOLID_H + +#include "pandabase.h" + +#include "collisionSolid.h" + +//////////////////////////////////////////////////////////////////// +// Class : CollisionDSSolid +// Description : A collision volume or object made up of the intersection +// of two spheres (potentially a lens) and two half-spaces +// (planes). +//////////////////////////////////////////////////////////////////// +class EXPCL_PANDA CollisionDSSolid: public CollisionSolid { +PUBLISHED: + INLINE CollisionDSSolid( + const LPoint3f ¢er_a, float radius_a, + const LPoint3f ¢er_b, float radius_b, + const Planef &plane_a, const Planef &plane_b); + INLINE CollisionDSSolid( + float ax, float ay, float az, float radius_a, + float bx, float by, float bz, float radius_b, + const Planef &plane_a, const Planef &plane_b); + +protected: + INLINE CollisionDSSolid(); + +public: + INLINE CollisionDSSolid(const CollisionDSSolid ©); + virtual CollisionSolid *make_copy(); + + virtual void xform(const LMatrix4f &mat); + virtual LPoint3f get_collision_origin() const; + + virtual PStatCollector get_volume_pcollector(); + virtual PStatCollector get_test_pcollector(); + + virtual void output(ostream &out) const; + +PUBLISHED: + // Sphere a + INLINE void set_center_a(const LPoint3f ¢er); + INLINE void set_center_a(float x, float y, float z); + INLINE const LPoint3f &get_center_a() const; + + INLINE void set_radius_a(float radius); + INLINE float get_radius_a() const; + + // Sphere b + INLINE void set_center_b(const LPoint3f ¢er); + INLINE void set_center_b(float x, float y, float z); + INLINE const LPoint3f &get_center_b() const; + + INLINE void set_radius_b(float radius); + INLINE float get_radius_b() const; + + // Plane a + INLINE LVector3f get_normal_a() const; + INLINE float dist_to_plane_a(const LPoint3f &point) const; + + INLINE void set_plane_a(const Planef &plane); + INLINE const Planef &get_plane_a() const; + + // Plane b + INLINE LVector3f get_normal_b() const; + INLINE float dist_to_plane_b(const LPoint3f &point) const; + + INLINE void set_plane_b(const Planef &plane); + INLINE const Planef &get_plane_b() const; + +protected: + virtual PT(BoundingVolume) compute_internal_bounds() const; + + virtual PT(CollisionEntry) + test_intersection_from_sphere(const CollisionEntry &entry) const; + + virtual void fill_viz_geom(); + +protected: + void recalc_internals(); + Vertexf compute_point(float latitude, float longitude) const; + +private: + LPoint3f _center_a; + float _radius_a; + LPoint3f _center_b; + float _radius_b; + Planef _plane_a; + Planef _plane_b; + + static PStatCollector _volume_pcollector; + static PStatCollector _test_pcollector; + + Vertexf calc_sphere1_vertex( + int ri, int si, int num_rings, int num_slices, float length, + float angle); + Vertexf calc_sphere2_vertex( + int ri, int si, int num_rings, int num_slices, float length, + float angle); + +public: + static void register_with_read_factory(); + virtual void write_datagram(BamWriter *manager, Datagram &me); + +protected: + static TypedWritable *make_CollisionSphere(const FactoryParams ¶ms); + static TypedWritable *make_from_bam(const FactoryParams ¶ms); + void fillin(DatagramIterator &scan, BamReader *manager); + +public: + static TypeHandle get_class_type() { + return _type_handle; + } + static void init_type() { + CollisionSolid::init_type(); + register_type(_type_handle, "CollisionDSSolid", + CollisionSolid::get_class_type()); + } + virtual TypeHandle get_type() const { + return get_class_type(); + } + virtual TypeHandle force_init_type() {init_type(); return get_class_type();} + +private: + static TypeHandle _type_handle; +}; + +#include "collisionDSSolid.I" + +#endif