16 #include "collisionSphere.h" 17 #include "collisionLine.h" 18 #include "collisionRay.h" 19 #include "collisionSegment.h" 20 #include "collisionHandler.h" 21 #include "collisionEntry.h" 22 #include "collisionParabola.h" 23 #include "config_collide.h" 24 #include "boundingSphere.h" 26 #include "datagramIterator.h" 27 #include "bamReader.h" 28 #include "bamWriter.h" 29 #include "nearly_zero.h" 31 #include "mathNumbers.h" 33 #include "geomTristrips.h" 34 #include "geomVertexWriter.h" 37 "Collision Volumes:CollisionSphere");
39 "Collision Tests:CollisionSphere");
59 return entry.
get_into()->test_intersection_from_sphere(entry);
67 void CollisionSphere::
69 _center = _center * mat;
74 _radius = length(radius_v);
76 mark_internal_bounds_stale();
101 return _volume_pcollector;
113 return _test_pcollector;
121 void CollisionSphere::
122 output(ostream &out)
const {
123 out <<
"sphere, c (" << get_center() <<
"), r " << get_radius();
132 compute_internal_bounds()
const {
142 test_intersection_from_sphere(
const CollisionEntry &entry)
const {
144 DCAST_INTO_R(sphere, entry.
get_from(), NULL);
146 CPT(TransformState) wrt_space = entry.get_wrt_space();
148 const LMatrix4 &wrt_mat = wrt_space->get_mat();
150 LPoint3 from_b = sphere->get_center() * wrt_mat;
152 LPoint3 into_center(get_center());
153 PN_stdfloat into_radius(get_radius());
156 LVector3(sphere->get_radius(), 0.0f, 0.0f) * wrt_mat;
157 PN_stdfloat from_radius = length(from_radius_v);
159 LPoint3 into_intersection_point(from_b);
161 LPoint3 contact_point(into_intersection_point);
162 PN_stdfloat actual_t = 0.0f;
164 LVector3 vec = from_b - into_center;
165 PN_stdfloat dist2 = dot(vec, vec);
166 if (dist2 > (into_radius + from_radius) * (into_radius + from_radius)) {
169 CPT(TransformState) wrt_prev_space = entry.get_wrt_prev_space();
170 LPoint3 from_a = sphere->get_center() * wrt_prev_space->get_mat();
173 LVector3 from_direction = from_b - from_a;
174 if (!intersects_line(t1, t2, from_a, from_direction, from_radius)) {
179 if (t2 < 0.0 || t1 > 1.0) {
186 actual_t = min(1.0, max(0.0, t1));
187 contact_point = from_a + actual_t * (from_b - from_a);
192 into_intersection_point = from_a;
196 into_intersection_point = from_a + t1 * from_direction;
204 if (collide_cat.is_debug()) {
211 LPoint3 from_center = sphere->get_center() * wrt_mat;
214 LVector3 v(into_intersection_point - into_center);
215 PN_stdfloat vec_length = v.
length();
216 if (IS_NEARLY_ZERO(vec_length)) {
220 surface_normal.set(1.0, 0.0, 0.0);
222 surface_normal = v / vec_length;
228 LVector3 v2 = contact_point - into_center;
229 PN_stdfloat v2_len = v2.
length();
230 if (IS_NEARLY_ZERO(v2_len)) {
234 contact_normal.set(1.0, 0.0, 0.0);
236 contact_normal = v2 / v2_len;
239 new_entry->set_surface_normal(eff_normal);
240 new_entry->set_surface_point(into_center + surface_normal * into_radius);
241 new_entry->set_interior_point(from_center - surface_normal * from_radius);
242 new_entry->set_contact_pos(contact_point);
243 new_entry->set_contact_normal(contact_normal);
244 new_entry->set_t(actual_t);
257 DCAST_INTO_R(line, entry.
get_from(), NULL);
259 const LMatrix4 &wrt_mat = entry.get_wrt_mat();
261 LPoint3 from_origin = line->get_origin() * wrt_mat;
262 LVector3 from_direction = line->get_direction() * wrt_mat;
265 if (!intersects_line(t1, t2, from_origin, from_direction, 0.0f)) {
270 if (collide_cat.is_debug()) {
277 LPoint3 into_intersection_point = from_origin + t1 * from_direction;
278 new_entry->set_surface_point(into_intersection_point);
283 LVector3 normal = into_intersection_point - get_center();
285 new_entry->set_surface_normal(normal);
299 DCAST_INTO_R(box, entry.
get_from(), NULL);
303 const LMatrix4 &wrt_mat = entry.get_inv_wrt_mat();
305 LPoint3 center = wrt_mat.xform_point(_center);
308 LPoint3 box_min = box->get_min();
309 LPoint3 box_max = box->get_max();
315 if (center[0] < box_min[0]) {
316 s = center[0] - box_min[0];
319 }
else if (center[0] > box_max[0]) {
320 s = center[0] - box_max[0];
324 if (center[1] < box_min[1]) {
325 s = center[1] - box_min[1];
328 }
else if (center[1] > box_max[1]) {
329 s = center[1] - box_max[1];
333 if (center[2] < box_min[2]) {
334 s = center[2] - box_min[2];
337 }
else if (center[2] > box_max[2]) {
338 s = center[2] - box_max[2];
346 if (collide_cat.is_debug()) {
355 LPoint3 interior = entry.get_wrt_mat().
xform_point(center.fmax(box_min).fmin(box_max));
356 new_entry->set_interior_point(interior);
359 LVector3 normal = interior - _center;
361 new_entry->set_surface_point(_center + normal * _radius);
362 new_entry->set_surface_normal(
377 DCAST_INTO_R(ray, entry.
get_from(), NULL);
379 const LMatrix4 &wrt_mat = entry.get_wrt_mat();
381 LPoint3 from_origin = ray->get_origin() * wrt_mat;
382 LVector3 from_direction = ray->get_direction() * wrt_mat;
385 if (!intersects_line(t1, t2, from_origin, from_direction, 0.0f)) {
397 if (collide_cat.is_debug()) {
404 LPoint3 into_intersection_point = from_origin + t1 * from_direction;
405 new_entry->set_surface_point(into_intersection_point);
410 LVector3 normal = into_intersection_point - get_center();
412 new_entry->set_surface_normal(normal);
424 test_intersection_from_segment(
const CollisionEntry &entry)
const {
426 DCAST_INTO_R(segment, entry.
get_from(), NULL);
428 const LMatrix4 &wrt_mat = entry.get_wrt_mat();
430 LPoint3 from_a = segment->get_point_a() * wrt_mat;
431 LPoint3 from_b = segment->get_point_b() * wrt_mat;
432 LVector3 from_direction = from_b - from_a;
435 if (!intersects_line(t1, t2, from_a, from_direction, 0.0f)) {
440 if (t2 < 0.0 || t1 > 1.0) {
448 if (collide_cat.is_debug()) {
455 LPoint3 into_intersection_point = from_a + t1 * from_direction;
456 new_entry->set_surface_point(into_intersection_point);
461 LVector3 normal = into_intersection_point - get_center();
463 new_entry->set_surface_normal(normal);
475 test_intersection_from_parabola(
const CollisionEntry &entry)
const {
477 DCAST_INTO_R(parabola, entry.
get_from(), NULL);
479 const LMatrix4 &wrt_mat = entry.get_wrt_mat();
483 local_p.
xform(wrt_mat);
486 if (!intersects_parabola(t, local_p, parabola->
get_t1(), parabola->
get_t2(),
487 local_p.calc_point(parabola->
get_t1()),
488 local_p.calc_point(parabola->
get_t2()))) {
493 if (collide_cat.is_debug()) {
500 LPoint3 into_intersection_point = local_p.calc_point(t);
501 new_entry->set_surface_point(into_intersection_point);
506 LVector3 normal = into_intersection_point - get_center();
508 new_entry->set_surface_normal(normal);
520 void CollisionSphere::
522 if (collide_cat.is_debug()) {
524 <<
"Recomputing viz for " << *
this <<
"\n";
527 static const int num_slices = 16;
528 static const int num_stacks = 8;
531 (
"collision", GeomVertexFormat::get_v3(),
536 for (
int sl = 0; sl < num_slices; ++sl) {
537 PN_stdfloat longitude0 = (PN_stdfloat)sl / (PN_stdfloat)num_slices;
538 PN_stdfloat longitude1 = (PN_stdfloat)(sl + 1) / (PN_stdfloat)num_slices;
539 vertex.
add_data3(compute_point(0.0, longitude0));
540 for (
int st = 1; st < num_stacks; ++st) {
541 PN_stdfloat latitude = (PN_stdfloat)st / (PN_stdfloat)num_stacks;
542 vertex.
add_data3(compute_point(latitude, longitude0));
543 vertex.
add_data3(compute_point(latitude, longitude1));
545 vertex.
add_data3(compute_point(1.0, longitude0));
547 strip->add_next_vertices(num_stacks * 2);
548 strip->close_primitive();
552 geom->add_primitive(strip);
554 _viz_geom->add_geom(geom, get_solid_viz_state());
555 _bounds_viz_geom->add_geom(geom, get_solid_bounds_viz_state());
571 bool CollisionSphere::
572 intersects_line(
double &t1,
double &t2,
574 PN_stdfloat inflate_radius)
const {
605 double A = dot(delta, delta);
607 nassertr(A != 0.0,
false);
610 double B = 2.0f* dot(delta, fc);
611 double fc_d2 = dot(fc, fc);
612 double radius = get_radius() + inflate_radius;
613 double C = fc_d2 - radius * radius;
615 double radical = B*B - 4.0*A*C;
617 if (IS_NEARLY_ZERO(radical)) {
619 t1 = t2 = -B /(2.0*A);
622 }
else if (radical < 0.0) {
627 double reciprocal_2A = 1.0/(2.0*A);
628 double sqrt_radical = sqrtf(radical);
629 t1 = ( -B - sqrt_radical ) * reciprocal_2A;
630 t2 = ( -B + sqrt_radical ) * reciprocal_2A;
648 bool CollisionSphere::
649 intersects_parabola(
double &t,
const LParabola ¶bola,
650 double t1,
double t2,
654 if ((p1 - _center).length_squared() > _radius * _radius) {
674 double tmid = (t1 + t2) * 0.5;
675 if (tmid != t1 && tmid != t2) {
676 LPoint3 pmid = parabola.calc_point(tmid);
677 LPoint3 pmid2 = (p1 + p2) * 0.5f;
679 if ((pmid - pmid2).length_squared() > 0.001f) {
681 if (intersects_parabola(t, parabola, t1, tmid, p1, pmid)) {
684 return intersects_parabola(t, parabola, tmid, t2, pmid, p2);
690 if (!intersects_line(t1a, t2a, p1, p2 - p1, 0.0f)) {
694 if (t2a < 0.0 || t1a > 1.0) {
710 LVertex CollisionSphere::
711 compute_point(PN_stdfloat latitude, PN_stdfloat longitude)
const {
713 csincos(latitude * MathNumbers::pi, &s1, &c1);
716 csincos(longitude * 2.0f * MathNumbers::pi, &s2, &c2);
718 LVertex p(s1 * c2, s1 * s2, c1);
719 return p * get_radius() + get_center();
756 parse_params(params, scan, manager);
757 me->fillin(scan, manager);
769 void CollisionSphere::
771 CollisionSolid::fillin(scan, manager);
float length_squared() const
Returns the square of the vector's length, cheap and easy.
const LParabola & get_parabola() const
Returns the parabola specified by this solid.
PN_stdfloat get_t1() const
Returns the starting point on the parabola.
const LVector3 & get_effective_normal() const
Returns the normal that was set by set_effective_normal().
This object provides a high-level interface for quickly writing a sequence of numeric values from a v...
An infinite ray, with a specific origin and direction.
PN_stdfloat get_stdfloat()
Extracts either a 32-bit or a 64-bit floating-point number, according to Datagram::set_stdfloat_doubl...
virtual PStatCollector & get_test_pcollector()
Returns a PStatCollector that is used to count the number of intersection tests made against a solid ...
NodePath get_into_node_path() const
Returns the NodePath that represents the specific CollisionNode or GeomNode instance that was collide...
virtual void write_datagram(BamWriter *manager, Datagram &me)
Function to write the important information in the particular object to a Datagram.
virtual LPoint3 get_collision_origin() const
Returns the point in space deemed to be the "origin" of the solid for collision purposes.
This is the fundamental interface for extracting binary objects from a Bam file, as generated by a Ba...
static void register_with_read_factory()
Factory method to generate a CollisionSphere object.
PN_stdfloat get_t2() const
Returns the ending point on the parabola.
void read_datagram(DatagramIterator &source)
Reads the vector from the Datagram using get_stdfloat().
The abstract base class for all things that can collide with other things in the world, and all the things they can collide with (except geometry).
virtual PStatCollector & get_volume_pcollector()
Returns a PStatCollector that is used to count the number of bounding volume tests made against a sol...
This defines a bounding sphere, consisting of a center and a radius.
A cuboid collision volume or object.
Base class for objects that can be written to and read from Bam files.
This is a three-component vector distance (as opposed to a three-component point, which represents a ...
Defines a series of triangle strips.
This is a three-component point in space (as opposed to a three-component vector, which represents a ...
This is the fundamental interface for writing binary objects to a Bam file, to be extracted later by ...
A spherical collision volume or object.
virtual void write_datagram(BamWriter *manager, Datagram &me)
Function to write the important information in the particular object to a Datagram.
bool almost_equal(const LVecBase3f &other, float threshold) const
Returns true if two vectors are memberwise equal within a specified tolerance.
void add_stdfloat(PN_stdfloat value)
Adds either a 32-bit or a 64-bit floating-point number, according to set_stdfloat_double().
This is an abstract class for any volume in any sense which can be said to define the locality of ref...
A lightweight class that represents a single element that may be timed and/or counted via stats...
A finite line segment, with two specific endpoints but no thickness.
const CollisionSolid * get_into() const
Returns the CollisionSolid pointer for the particular solid was collided into.
LVecBase3f xform_point(const LVecBase3f &v) const
The matrix transforms a 3-component point (including translation component) and returns the result...
This is a 4-by-4 transform matrix.
void write_datagram(Datagram &destination) const
Writes the vector to the Datagram using add_stdfloat().
Defines a single collision event.
This defines the actual numeric vertex data stored in a Geom, in the structure defined by a particula...
A container for geometry primitives.
An instance of this class is passed to the Factory when requesting it to do its business and construc...
void add_data3(PN_stdfloat x, PN_stdfloat y, PN_stdfloat z)
Sets the write row to a particular 3-component value, and advances the write row. ...
LVecBase4f xform(const LVecBase4f &v) const
4-component vector or point times matrix.
void register_factory(TypeHandle handle, CreateFunc *func)
Registers a new kind of thing the Factory will be able to create.
An infinite line, similar to a CollisionRay, except that it extends in both directions.
float length() const
Returns the length of the vector, by the Pythagorean theorem.
static WritableFactory * get_factory()
Returns the global WritableFactory for generating TypedWritable objects.
This defines a parabolic arc, or subset of an arc, similar to the path of a projectile or falling obj...
bool has_effective_normal() const
Returns true if a special normal was set by set_effective_normal(), false otherwise.
A class to retrieve the individual data elements previously stored in a Datagram. ...
NodePath get_from_node_path() const
Returns the NodePath that represents the CollisionNode that contains the CollisionSolid that triggere...
TypeHandle is the identifier used to differentiate C++ class types.
bool get_respect_effective_normal() const
See set_respect_effective_normal().
bool normalize()
Normalizes the vector in place.
An ordered list of data elements, formatted in memory for transmission over a socket or writing to a ...
const CollisionSolid * get_from() const
Returns the CollisionSolid pointer for the particular solid that triggered this collision.