collisionCone.cxx

Go to the documentation of this file.

/**
 * PANDA 3D SOFTWARE
 * Copyright (c) Carnegie Mellon University.  All rights reserved.
 *
 * All use of this software is subject to the terms of the revised BSD
 * license.  You should have received a copy of this license along
 * with this source code in a file named "LICENSE."
 *
 * @file collisionCone.cxx
 * @author rdb
 * @date 2021-01-29
 */
 
#include "collisionCone.h"
#include "collisionEntry.h"
#include "collisionSphere.h"
#include "collisionPolygon.h"
#include "config_collide.h"
#include "cmath.h"
#include "geom.h"
#include "geomTrifans.h"
#include "geomLinestrips.h"
#include "mathNumbers.h"
#include "billboardEffect.h"
 
PStatCollector CollisionCone::_volume_pcollector("Collision Volumes:CollisionCone");
PStatCollector CollisionCone::_test_pcollector("Collision Tests:CollisionCone");
TypeHandle CollisionCone::_type_handle;
 
/**
 *
 */
CollisionSolid *CollisionCone::
make_copy() {
  return new CollisionCone(*this);
}
 
/**
 * Transforms the solid by the indicated matrix.
 */
void CollisionCone::
xform(const LMatrix4 &mat) {
  CollisionSolid::xform(mat);
}
 
/**
 * 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.
 */
LPoint3 CollisionCone::
get_collision_origin() const {
  return LPoint3::origin();
}
 
/**
 * 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 &CollisionCone::
get_volume_pcollector() {
  return _volume_pcollector;
}
 
/**
 * 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 &CollisionCone::
get_test_pcollector() {
  return _test_pcollector;
}
 
/**
 *
 */
void CollisionCone::
output(std::ostream &out) const {
  out << "ccone, (" << _radius << ")";
}
 
/**
 *
 */
PT(BoundingVolume) CollisionCone::
compute_internal_bounds() const {
  return new BoundingSphere(LPoint3(0, 0, 0), std::max(_radius, _apex[2]) * 2);
}
 
/**
 *
 */
PT(CollisionEntry) CollisionCone::
test_intersection_from_sphere(const CollisionEntry &entry) const {
  const CollisionSphere *sphere;
  DCAST_INTO_R(sphere, entry.get_from(), nullptr);
 
  const LMatrix4 &wrt_mat = entry.get_wrt_mat();
 
  LPoint3 from_center = sphere->get_center() * wrt_mat;
  LVector3 from_radius_v =
    LVector3(sphere->get_radius(), 0.0f, 0.0f) * wrt_mat;
  PN_stdfloat from_radius = length(from_radius_v);
 
  // Distance to center line
  PN_stdfloat dist_to_line = from_center.get_xy().length();
  if (dist_to_line > _radius + from_radius) {
    return nullptr;
  }
 
  if (from_center.get_z() > _apex.get_z() + from_radius) {
    return nullptr;
  }
 
  LPlane side(LPoint3(_radius, 1, 0), LPoint3(_radius, 0, 0), _apex);
 
  PN_stdfloat dist_bottom = from_center.get_z() + from_radius; 
  PN_stdfloat dist_side = side.dist_to_plane(LPoint3(dist_to_line, 0, from_center.get_z())) + from_radius;
 
  std::cerr << dist_side << " " << dist_to_line << "\n";
 
 
  if (dist_bottom < 0 || dist_side < 0) {
    return nullptr;
  }
 
  if (collide_cat.is_debug()) {
    collide_cat.debug()
      << "intersection detected from " << entry.get_from_node_path()
      << " into " << entry.get_into_node_path() << "\n";
  }
  PT(CollisionEntry) new_entry = new CollisionEntry(entry);
 
  LPoint3 surface_point;
  LVector3 surface_normal;
 
  if (dist_bottom < dist_side) {
    surface_normal = LVector3(0, 0, -1);
    surface_point = LPoint3(from_center[0], from_center[1], 0);
    if (dist_to_line > _radius) {
      surface_point = surface_point * (_radius / dist_to_line);
    }
  }
  else {
    surface_normal = side.get_normal();
    surface_point = get_apex();
  }
 
  LVector3 normal = (
    has_effective_normal() && sphere->get_respect_effective_normal())
    ? get_effective_normal() : surface_normal;
//
  new_entry->set_surface_normal(normal);
  new_entry->set_surface_point(surface_point);
  //new_entry->set_interior_point(from_center - get_normal() * from_radius);
 
  return new_entry;
}
 
/**
 * Fills the _viz_geom GeomNode up with Geoms suitable for rendering this
 * solid.
 */
void CollisionCone::
fill_viz_geom() {
  if (collide_cat.is_debug()) {
    collide_cat.debug()
      << "Recomputing viz for " << *this << "\n";
  }
 
  static int num_base_vertices = 8;
 
  PT(GeomVertexData) vdata = new GeomVertexData
    ("collision", GeomVertexFormat::get_v3(),
     Geom::UH_static);
  vdata->unclean_set_num_rows(num_base_vertices + 2);
 
  {
    GeomVertexWriter vertex(vdata, InternalName::get_vertex());
    double factor = MathNumbers::pi * 2.0 / (double)num_base_vertices;
    for (int i = 0; i < num_base_vertices; ++i) {
      double angle = i * factor;
      vertex.set_data3(ccos(angle) * _radius, csin(angle) * _radius, 0);
    }
    vertex.set_data3(0, 0, 0);
    vertex.set_data3(get_apex());
  }
 
  PT(GeomTrifans) body = new GeomTrifans(Geom::UH_static);
  body->add_vertex(num_base_vertices);
  body->add_vertex(0);
  for (int i = num_base_vertices - 1; i >= 0; --i) {
    body->add_vertex(i);
  }
  body->close_primitive();
 
  body->add_vertex(num_base_vertices + 1);
  body->add_consecutive_vertices(0, num_base_vertices);
  body->add_vertex(0);
  body->close_primitive();
 
  PT(GeomLinestrips) border = new GeomLinestrips(Geom::UH_static);
  border->add_consecutive_vertices(0, num_base_vertices);
  border->add_vertex(0);
  border->add_vertex(num_base_vertices + 1);
  border->add_vertex(num_base_vertices / 2);
  border->close_primitive();
 
  PT(Geom) geom1 = new Geom(vdata);
  geom1->add_primitive(body);
 
  PT(Geom) geom2 = new Geom(vdata);
  geom2->add_primitive(border);
 
  _viz_geom->add_geom(geom1, get_solid_viz_state());
  _viz_geom->add_geom(geom2, get_wireframe_viz_state());
  _viz_geom->set_effect(BillboardEffect::make_axis());
 
  _bounds_viz_geom->add_geom(geom1, get_solid_bounds_viz_state());
  _bounds_viz_geom->add_geom(geom2, get_wireframe_bounds_viz_state());
  _bounds_viz_geom->set_effect(BillboardEffect::make_axis());
}
 
/**
 * Function to write the important information in the particular object to a
 * Datagram
 */
void CollisionCone::
write_datagram(BamWriter *manager, Datagram &me) {
  CollisionSolid::write_datagram(manager, me);
  //_plane.write_datagram(me);
}
 
/**
 * Function that reads out of the datagram (or asks manager to read) all of
 * the data that is needed to re-create this object and stores it in the
 * appropiate place
 */
void CollisionCone::
fillin(DatagramIterator& scan, BamReader* manager)
{
  CollisionSolid::fillin(scan, manager);
  //_plane.read_datagram(scan);
}
 
/**
 * Factory method to generate a CollisionCone object
 */
TypedWritable *CollisionCone::
make_from_bam(const FactoryParams &params) {
  CollisionCone *me = new CollisionCone;
  DatagramIterator scan;
  BamReader *manager;
 
  parse_params(params, scan, manager);
  me->fillin(scan, manager);
  return me;
}
 
/**
 * Factory method to generate a CollisionCone object
 */
void CollisionCone::
register_with_read_factory() {
  BamReader::get_factory()->register_factory(get_class_type(), make_from_bam);
}