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);
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(), 0);
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();
172 if (!from_a.almost_equal(from_b)) {
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()) {
206 <<
"intersection detected from " << entry.get_from_node_path()
207 <<
" into " << entry.get_into_node_path() <<
"\n";
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(), 0);
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()) {
272 <<
"intersection detected from " << entry.get_from_node_path()
273 <<
" into " << entry.get_into_node_path() <<
"\n";
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(), 0);
301 CPT(TransformState) wrt_space = entry.get_wrt_space();
302 CPT(TransformState) wrt_prev_space = entry.get_wrt_prev_space();
304 const
LMatrix4 &wrt_mat = wrt_space->get_mat();
307 local_b.
xform( wrt_mat );
309 LPoint3 from_center = local_b.get_center();
311 LPoint3 orig_center = get_center();
312 LPoint3 to_center = orig_center;
313 LPoint3 contact_point(from_center);
314 PN_stdfloat actual_t = 1.0f;
316 PN_stdfloat to_radius = get_radius();
317 PN_stdfloat to_radius_2 = to_radius * to_radius;
320 PN_stdfloat max_dist = 0.0f;
321 PN_stdfloat dist = 0.0f;
326 for (ip = 0, intersect=false; ip < 6 && !intersect; ip++) {
327 plane = local_b.get_plane( ip );
328 if (local_b.get_plane_points(ip).size() < 3) {
348 if (!plane.intersects_line(dist, to_center, -(plane.get_normal()))) {
354 if (dist > to_radius || dist < -to_radius) {
359 LPoint2 p = local_b.to_2d(to_center - dist * plane.get_normal(), ip);
360 PN_stdfloat edge_dist = 0.0f;
362 edge_dist = local_b.dist_to_polygon(p, local_b.get_plane_points(ip));
369 if((edge_dist > 0) &&
370 ((edge_dist * edge_dist + dist * dist) > to_radius_2)) {
381 max_dist = to_radius;
382 if (edge_dist >= 0.0f) {
383 PN_stdfloat max_dist_2 = max(to_radius_2 - edge_dist * edge_dist, (PN_stdfloat)0.0);
384 max_dist = csqrt(max_dist_2);
387 if (dist > max_dist) {
396 if (collide_cat.is_debug()) {
398 <<
"intersection detected from " << entry.get_from_node_path()
399 <<
" into " << entry.get_into_node_path() <<
"\n";
404 PN_stdfloat into_depth = max_dist - dist;
406 new_entry->set_surface_normal(normal);
407 new_entry->set_surface_point(to_center - normal * dist);
408 new_entry->set_interior_point(to_center - normal * (dist + into_depth));
409 new_entry->set_contact_pos(contact_point);
410 new_entry->set_contact_normal(plane.get_normal());
411 new_entry->set_t(actual_t);
424 DCAST_INTO_R(ray, entry.get_from(), 0);
426 const LMatrix4 &wrt_mat = entry.get_wrt_mat();
428 LPoint3 from_origin = ray->get_origin() * wrt_mat;
429 LVector3 from_direction = ray->get_direction() * wrt_mat;
432 if (!intersects_line(t1, t2, from_origin, from_direction, 0.0f)) {
444 if (collide_cat.is_debug()) {
446 <<
"intersection detected from " << entry.get_from_node_path()
447 <<
" into " << entry.get_into_node_path() <<
"\n";
451 LPoint3 into_intersection_point = from_origin + t1 * from_direction;
452 new_entry->set_surface_point(into_intersection_point);
457 LVector3 normal = into_intersection_point - get_center();
459 new_entry->set_surface_normal(normal);
471 test_intersection_from_segment(const
CollisionEntry &entry)
const {
473 DCAST_INTO_R(segment, entry.get_from(), 0);
475 const LMatrix4 &wrt_mat = entry.get_wrt_mat();
477 LPoint3 from_a = segment->get_point_a() * wrt_mat;
478 LPoint3 from_b = segment->get_point_b() * wrt_mat;
479 LVector3 from_direction = from_b - from_a;
482 if (!intersects_line(t1, t2, from_a, from_direction, 0.0f)) {
487 if (t2 < 0.0 || t1 > 1.0) {
495 if (collide_cat.is_debug()) {
497 <<
"intersection detected from " << entry.get_from_node_path()
498 <<
" into " << entry.get_into_node_path() <<
"\n";
502 LPoint3 into_intersection_point = from_a + t1 * from_direction;
503 new_entry->set_surface_point(into_intersection_point);
508 LVector3 normal = into_intersection_point - get_center();
510 new_entry->set_surface_normal(normal);
522 test_intersection_from_parabola(const
CollisionEntry &entry)
const {
524 DCAST_INTO_R(parabola, entry.get_from(), 0);
526 const LMatrix4 &wrt_mat = entry.get_wrt_mat();
530 local_p.xform(wrt_mat);
533 if (!intersects_parabola(t, local_p, parabola->
get_t1(), parabola->
get_t2(),
534 local_p.calc_point(parabola->
get_t1()),
535 local_p.calc_point(parabola->
get_t2()))) {
540 if (collide_cat.is_debug()) {
542 <<
"intersection detected from " << entry.get_from_node_path()
543 <<
" into " << entry.get_into_node_path() <<
"\n";
547 LPoint3 into_intersection_point = local_p.calc_point(t);
548 new_entry->set_surface_point(into_intersection_point);
553 LVector3 normal = into_intersection_point - get_center();
555 new_entry->set_surface_normal(normal);
567 void CollisionSphere::
569 if (collide_cat.is_debug()) {
571 <<
"Recomputing viz for " << *
this <<
"\n";
574 static const int num_slices = 16;
575 static const int num_stacks = 8;
578 ("collision", GeomVertexFormat::get_v3(),
583 for (
int sl = 0; sl < num_slices; ++sl) {
584 PN_stdfloat longitude0 = (PN_stdfloat)sl / (PN_stdfloat)num_slices;
585 PN_stdfloat longitude1 = (PN_stdfloat)(sl + 1) / (PN_stdfloat)num_slices;
586 vertex.add_data3(compute_point(0.0, longitude0));
587 for (
int st = 1; st < num_stacks; ++st) {
588 PN_stdfloat latitude = (PN_stdfloat)st / (PN_stdfloat)num_stacks;
589 vertex.add_data3(compute_point(latitude, longitude0));
590 vertex.add_data3(compute_point(latitude, longitude1));
592 vertex.add_data3(compute_point(1.0, longitude0));
595 strip->close_primitive();
599 geom->add_primitive(strip);
601 _viz_geom->add_geom(geom, get_solid_viz_state());
602 _bounds_viz_geom->add_geom(geom, get_solid_bounds_viz_state());
619 intersects_line(
double &t1,
double &t2,
621 PN_stdfloat inflate_radius)
const {
652 double A = dot(delta, delta);
654 nassertr(A != 0.0,
false);
657 double B = 2.0f* dot(delta, fc);
658 double fc_d2 = dot(fc, fc);
659 double radius = get_radius() + inflate_radius;
660 double C = fc_d2 - radius * radius;
662 double radical = B*B - 4.0*A*C;
664 if (IS_NEARLY_ZERO(radical)) {
666 t1 = t2 = -B /(2.0*A);
669 }
else if (radical < 0.0) {
674 double reciprocal_2A = 1.0/(2.0*A);
675 double sqrt_radical = sqrtf(radical);
676 t1 = ( -B - sqrt_radical ) * reciprocal_2A;
677 t2 = ( -B + sqrt_radical ) * reciprocal_2A;
695 bool CollisionSphere::
696 intersects_parabola(
double &t,
const LParabola ¶bola,
697 double t1,
double t2,
701 if ((p1 - _center).length_squared() > _radius * _radius) {
721 double tmid = (t1 + t2) * 0.5;
722 if (tmid != t1 && tmid != t2) {
723 LPoint3 pmid = parabola.calc_point(tmid);
724 LPoint3 pmid2 = (p1 + p2) * 0.5f;
726 if ((pmid - pmid2).length_squared() > 0.001f) {
728 if (intersects_parabola(t, parabola, t1, tmid, p1, pmid)) {
731 return intersects_parabola(t, parabola, tmid, t2, pmid, p2);
737 if (!intersects_line(t1a, t2a, p1, p2 - p1, 0.0f)) {
741 if (t2a < 0.0 || t1a > 1.0) {
757 LVertex CollisionSphere::
758 compute_point(PN_stdfloat latitude, PN_stdfloat longitude)
const {
760 csincos(latitude * MathNumbers::pi, &s1, &c1);
763 csincos(longitude * 2.0f * MathNumbers::pi, &s2, &c2);
765 LVertex p(s1 * c2, s1 * s2, c1);
766 return p * get_radius() + get_center();
803 parse_params(params, scan, manager);
804 me->fillin(scan, manager);
816 void CollisionSphere::
818 CollisionSolid::fillin(scan, manager);
virtual void xform(const LMatrix4 &mat)
Transforms the solid by the indicated matrix.
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.
bool get_respect_effective_normal() const
See set_respect_effective_normal().
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 ...
virtual void write_datagram(BamWriter *manager, Datagram &me)
Function to write the important information in the particular object to a Datagram.
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.
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.
virtual LPoint3 get_collision_origin() const
Returns the point in space deemed to be the "origin" of the solid for collision purposes.
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.
const LVector3 & get_effective_normal() const
Returns the normal that was set by set_effective_normal().
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...
float length() const
Returns the length of the vector, by the Pythagorean theorem.
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.
This is a 4-by-4 transform matrix.
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...
PN_stdfloat get_t2() const
Returns the ending point on the parabola.
const LParabola & get_parabola() const
Returns the parabola specified by this solid.
void register_factory(TypeHandle handle, CreateFunc *func)
Registers a new kind of thing the Factory will be able to create.
void add_next_vertices(int num_vertices)
Adds the next n vertices in sequence, beginning from the last vertex added to the primitive + 1...
An infinite line, similar to a CollisionRay, except that it extends in both directions.
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...
PN_stdfloat get_t1() const
Returns the starting point on the parabola.
A class to retrieve the individual data elements previously stored in a Datagram. ...
This is a two-component point in space.
TypeHandle is the identifier used to differentiate C++ class types.
virtual void xform(const LMatrix4 &mat)
Transforms the solid by the indicated matrix.
bool normalize()
Normalizes the vector in place.
bool has_effective_normal() const
Returns true if a special normal was set by set_effective_normal(), false otherwise.
An ordered list of data elements, formatted in memory for transmission over a socket or writing to a ...
void write_datagram(Datagram &destination) const
Writes the vector to the Datagram using add_stdfloat().