diff --git a/panda/src/collide/collisionEntry.I b/panda/src/collide/collisionEntry.I index fd2b2d10ac..e9268d429b 100644 --- a/panda/src/collide/collisionEntry.I +++ b/panda/src/collide/collisionEntry.I @@ -275,25 +275,25 @@ has_interior_point() const { } //////////////////////////////////////////////////////////////////// -// Function: CollisionEntry::set_contact_point +// Function: CollisionEntry::set_contact_pos // Access: Published -// Description: Stores the point, on the surface of the "into" -// object, at which the collision is first detected. +// Description: Stores the position of the "from" object at the +// instant at which the collision is first detected. // -// This point is specified in the coordinate space of +// This position is specified in the coordinate space of // the "into" object. //////////////////////////////////////////////////////////////////// INLINE void CollisionEntry:: -set_contact_point(const LPoint3f &point) { - _contact_point = point; - _flags |= F_has_contact_point; +set_contact_pos(const LPoint3f &pos) { + _contact_pos = pos; + _flags |= F_has_contact_pos; } //////////////////////////////////////////////////////////////////// // Function: CollisionEntry::set_contact_normal // Access: Published // Description: Stores the surface normal of the "into" object at the -// contact point. +// contact pos. // // This normal is specified in the coordinate space of // the "into" object. @@ -305,16 +305,16 @@ set_contact_normal(const LVector3f &normal) { } //////////////////////////////////////////////////////////////////// -// Function: CollisionEntry::has_contact_point +// Function: CollisionEntry::has_contact_pos // Access: Published -// Description: Returns true if the contact point has been specified, -// false otherwise. See get_contact_point(). Some +// Description: Returns true if the contact position has been specified, +// false otherwise. See get_contact_pos(). Some // types of collisions may not compute the contact -// point. +// pos. //////////////////////////////////////////////////////////////////// INLINE bool CollisionEntry:: -has_contact_point() const { - return (_flags & F_has_contact_point) != 0; +has_contact_pos() const { + return (_flags & F_has_contact_pos) != 0; } //////////////////////////////////////////////////////////////////// diff --git a/panda/src/collide/collisionEntry.cxx b/panda/src/collide/collisionEntry.cxx index e0aefa7770..47aacc8424 100644 --- a/panda/src/collide/collisionEntry.cxx +++ b/panda/src/collide/collisionEntry.cxx @@ -41,7 +41,7 @@ CollisionEntry(const CollisionEntry ©) : _surface_point(copy._surface_point), _surface_normal(copy._surface_normal), _interior_point(copy._interior_point), - _contact_point(copy._contact_point), + _contact_pos(copy._contact_pos), _contact_normal(copy._contact_normal) { } @@ -65,7 +65,7 @@ operator = (const CollisionEntry ©) { _surface_point = copy._surface_point; _surface_normal = copy._surface_normal; _interior_point = copy._interior_point; - _contact_point = copy._contact_point; + _contact_pos = copy._contact_pos; _contact_normal = copy._contact_normal; } @@ -169,33 +169,26 @@ get_all(const NodePath &space, LPoint3f &surface_point, } //////////////////////////////////////////////////////////////////// -// Function: CollisionEntry::get_contact_point +// Function: CollisionEntry::get_contact_pos // Access: Published -// Description: Returns the point, on the surface of the "into" -// object, at which a collision is detected. This can -// be thought of as the first point of intersection. -// The surface point is not always the initial point of -// intersection. We preserve the original implementation -// of surface_point detection so that the existing -// collision response code will still work, and provide -// the contact_point for collision response code that -// needs precise collision information. +// Description: Returns the position of the "from" object at the instant +// that a collision is first detected. // -// The point will be converted into whichever coordinate +// The position will be converted into whichever coordinate // space the caller specifies. //////////////////////////////////////////////////////////////////// LPoint3f CollisionEntry:: -get_contact_point(const NodePath &space) const { - nassertr(has_contact_point(), LPoint3f::zero()); +get_contact_pos(const NodePath &space) const { + nassertr(has_contact_pos(), LPoint3f::zero()); CPT(TransformState) transform = _into_node_path.get_transform(space); - return _contact_point * transform->get_mat(); + return _contact_pos * transform->get_mat(); } //////////////////////////////////////////////////////////////////// // Function: CollisionEntry::get_contact_normal // Access: Published // Description: Returns the surface normal of the "into" object at -// the contact point. +// the contact position. // // The normal will be converted into whichever coordinate // space the caller specifies. @@ -210,25 +203,25 @@ get_contact_normal(const NodePath &space) const { //////////////////////////////////////////////////////////////////// // Function: CollisionEntry::get_all_contact_info // Access: Published -// Description: Simultaneously transforms the surface point, surface -// normal, and interior point of the collision into the +// Description: Simultaneously transforms the contact position and +// contact normal of the collision into the // indicated coordinate space. // // Returns true if all three properties are available, // or false if any one of them is not. //////////////////////////////////////////////////////////////////// bool CollisionEntry:: -get_all_contact_info(const NodePath &space, LPoint3f &contact_point, +get_all_contact_info(const NodePath &space, LPoint3f &contact_pos, LVector3f &contact_normal) const { CPT(TransformState) transform = _into_node_path.get_transform(space); const LMatrix4f &mat = transform->get_mat(); bool all_ok = true; - if (!has_contact_point()) { - contact_point = LPoint3f::zero(); + if (!has_contact_pos()) { + contact_pos = LPoint3f::zero(); all_ok = false; } else { - contact_point = _contact_point * mat; + contact_pos = _contact_pos * mat; } if (!has_contact_normal()) { diff --git a/panda/src/collide/collisionEntry.h b/panda/src/collide/collisionEntry.h index 3e51511dc1..704384caf6 100644 --- a/panda/src/collide/collisionEntry.h +++ b/panda/src/collide/collisionEntry.h @@ -78,10 +78,10 @@ PUBLISHED: INLINE bool has_surface_normal() const; INLINE bool has_interior_point() const; - INLINE void set_contact_point(const LPoint3f &point); + INLINE void set_contact_pos(const LPoint3f &pos); INLINE void set_contact_normal(const LVector3f &normal); - INLINE bool has_contact_point() const; + INLINE bool has_contact_pos() const; INLINE bool has_contact_normal() const; LPoint3f get_surface_point(const NodePath &space) const; @@ -92,10 +92,10 @@ PUBLISHED: LVector3f &surface_normal, LPoint3f &interior_point) const; - LPoint3f get_contact_point(const NodePath &space) const; + LPoint3f get_contact_pos(const NodePath &space) const; LVector3f get_contact_normal(const NodePath &space) const; bool get_all_contact_info(const NodePath &space, - LPoint3f &contact_point, + LPoint3f &contact_pos, LVector3f &contact_normal) const; void output(ostream &out) const; @@ -133,7 +133,7 @@ private: F_has_interior_point = 0x0004, F_respect_prev_transform = 0x0008, F_checked_clip_planes = 0x0010, - F_has_contact_point = 0x0020, + F_has_contact_pos = 0x0020, F_has_contact_normal = 0x0040, }; @@ -143,7 +143,7 @@ private: LVector3f _surface_normal; LPoint3f _interior_point; - LPoint3f _contact_point; + LPoint3f _contact_pos; LVector3f _contact_normal; public: diff --git a/panda/src/collide/collisionHandlerFluidPusher.cxx b/panda/src/collide/collisionHandlerFluidPusher.cxx index 63e560a130..2a507aa552 100755 --- a/panda/src/collide/collisionHandlerFluidPusher.cxx +++ b/panda/src/collide/collisionHandlerFluidPusher.cxx @@ -142,12 +142,12 @@ handle_entries() { Entries entries(*orig_entries); // this is the original position delta for the entire frame, before collision response - LPoint3f M(from_node_path.get_pos_delta(wrt_node)); + LVector3f M(from_node_path.get_pos_delta(wrt_node)); if (_horizontal) { M[2] = 0.0f; } // this is used to track position deltas every time we collide against a solid - LPoint3f N(M); + LVector3f N(M); collide_cat.info() << "N: " << N << endl; const LPoint3f orig_pos(from_node_path.get_pos(wrt_node)); @@ -198,7 +198,7 @@ handle_entries() { // iterate until the mover runs out of movement or gets stuck while (true) { - cout << "while (true)" << endl; + collide_cat.info() << "while (true)" << endl; const CollisionEntry *C = 0; // find the first (earliest) collision Entries::const_iterator cei; @@ -219,22 +219,22 @@ handle_entries() { collide_cat.info() << "t: " << C->get_t() << endl; - // move back to initial contact point - LPoint3f contact_point; + // move back to initial contact position + LPoint3f contact_pos; LVector3f contact_normal; - if (!C->get_all_contact_info(wrt_node, contact_point, contact_normal)) { + if (!C->get_all_contact_info(wrt_node, contact_pos, contact_normal)) { collide_cat.warning() << "Cannot shove on " << from_node_path << " for collision into " - << C->get_into_node_path() << "; no contact point/normal information.\n"; + << C->get_into_node_path() << "; no contact pos/normal information.\n"; break; } // calculate the position of the target node at the point of contact - contact_point -= sphere_offset; - collide_cat.info() << "contact_point: " << contact_point << endl; + contact_pos -= sphere_offset; + collide_cat.info() << "contact_pos: " << contact_pos << endl; uncollided_pos = candidate_final_pos; - candidate_final_pos = contact_point; + candidate_final_pos = contact_pos; LVector3f proj_surface_normal(contact_normal); if (_horizontal) { @@ -284,16 +284,24 @@ handle_entries() { } } - LVector3f blocked_movement(uncollided_pos - contact_point); + LVector3f blocked_movement(uncollided_pos - contact_pos); if (_horizontal) { blocked_movement[2] = 0.0f; } collide_cat.info() << "blocked movement: " << blocked_movement << endl; + float push_magnitude(-blocked_movement.dot(proj_surface_normal)); + LVector3f push; + if (push_magnitude < 0.0f) { + // don't ever push into plane, always push out along plane normal + push = LVector3f(0,0,0); + } else { + push = norm_proj_surface_normal * push_magnitude; + } + // calculate new position given that you collided with this thing // project the final position onto the plane of the obstruction - candidate_final_pos = uncollided_pos + (norm_proj_surface_normal * - -blocked_movement.dot(proj_surface_normal)); + candidate_final_pos = uncollided_pos + push; collide_cat.info() << "candidate_final_pos: " << candidate_final_pos << endl; @@ -303,8 +311,8 @@ handle_entries() { from_node_path.set_pos(wrt_node, candidate_final_pos); CPT(TransformState) prev_trans(from_node_path.get_prev_transform(wrt_node)); collide_cat.info() << "prev_trans->get_pos: " << prev_trans->get_pos() << endl; - prev_trans = prev_trans->set_pos(contact_point); - collide_cat.info() << "contact_point: " << contact_point << endl; + prev_trans = prev_trans->set_pos(contact_pos); + collide_cat.info() << "contact_pos: " << contact_pos << endl; collide_cat.info() << "prev_trans->get_pos: " << prev_trans->get_pos() << endl; from_node_path.set_prev_transform(wrt_node, prev_trans); diff --git a/panda/src/collide/collisionInvSphere.cxx b/panda/src/collide/collisionInvSphere.cxx index 5936c4797c..f7383ba345 100644 --- a/panda/src/collide/collisionInvSphere.cxx +++ b/panda/src/collide/collisionInvSphere.cxx @@ -175,7 +175,7 @@ test_intersection_from_line(const CollisionEntry &entry) const { LVector3f from_direction = line->get_direction() * wrt_mat; double t1, t2; - if (!intersects_line(t1, t2, from_origin, from_direction)) { + if (!intersects_line(t1, t2, from_origin, from_direction, 0.0f)) { // The line is in the middle of space, and therefore intersects // the sphere. t1 = t2 = 0.0; @@ -218,7 +218,7 @@ test_intersection_from_ray(const CollisionEntry &entry) const { LVector3f from_direction = ray->get_direction() * wrt_mat; double t1, t2; - if (!intersects_line(t1, t2, from_origin, from_direction)) { + if (!intersects_line(t1, t2, from_origin, from_direction, 0.0f)) { // The ray is in the middle of space, and therefore intersects // the sphere. t1 = t2 = 0.0; @@ -265,7 +265,7 @@ test_intersection_from_segment(const CollisionEntry &entry) const { LVector3f from_direction = from_b - from_a; double t1, t2; - if (!intersects_line(t1, t2, from_a, from_direction)) { + if (!intersects_line(t1, t2, from_a, from_direction, 0.0f)) { // The segment is in the middle of space, and therefore intersects // the sphere. t1 = t2 = 0.0; diff --git a/panda/src/collide/collisionPolygon.cxx b/panda/src/collide/collisionPolygon.cxx index f57a005564..a6c1b4c231 100644 --- a/panda/src/collide/collisionPolygon.cxx +++ b/panda/src/collide/collisionPolygon.cxx @@ -571,7 +571,7 @@ test_intersection_from_sphere(const CollisionEntry &entry) const { new_entry->set_surface_normal(normal); new_entry->set_surface_point(from_center - normal * dist); new_entry->set_interior_point(from_center - normal * (dist + into_depth)); - new_entry->set_contact_point(contact_point); + new_entry->set_contact_pos(contact_point); new_entry->set_contact_normal(get_normal()); new_entry->set_t(actual_t); diff --git a/panda/src/collide/collisionSphere.cxx b/panda/src/collide/collisionSphere.cxx index c083a1bd97..05d22bf5d3 100644 --- a/panda/src/collide/collisionSphere.cxx +++ b/panda/src/collide/collisionSphere.cxx @@ -315,21 +315,57 @@ test_intersection_from_sphere(const CollisionEntry &entry) const { const CollisionSphere *sphere; DCAST_INTO_R(sphere, entry.get_from(), 0); - const LMatrix4f &wrt_mat = entry.get_wrt_mat(); + CPT(TransformState) wrt_space = entry.get_wrt_space(); + CPT(TransformState) wrt_prev_space = entry.get_wrt_prev_space(); + + const LMatrix4f &wrt_mat = wrt_space->get_mat(); + + LPoint3f from_a = sphere->get_center() * wrt_prev_space->get_mat(); + LPoint3f from_b = sphere->get_center() * wrt_mat; + + LPoint3f into_center(get_center()); + float into_radius(get_radius()); - 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 into_center = get_center(); - float into_radius = get_radius(); + LPoint3f into_intersection_point; + double t1, t2; - LVector3f vec = from_center - into_center; - float dist2 = dot(vec, vec); - if (dist2 > (into_radius + from_radius) * (into_radius + from_radius)) { - // No intersection. - return NULL; + if (from_a != from_b) { + LVector3f from_direction = from_b - from_a; + if (!intersects_line(t1, t2, from_a, from_direction, from_radius)) { + // No intersection. + return NULL; + } + + if (t2 < 0.0 || t1 > 1.0) { + // Both intersection points are before the start of the segment or + // after the end of the segment. + return NULL; + } + + if (t1 < 0.0) { + // Point a is within the sphere. The first intersection point is + // point a itself. + into_intersection_point = from_a; + } else { + // Point a is outside the sphere, and point b is either inside the + // sphere or beyond it. The first intersection point is at t1. + into_intersection_point = from_a + t1 * from_direction; + } + + } else { + LVector3f vec = from_a - into_center; + float dist2 = dot(vec, vec); + if (dist2 > (into_radius + from_radius) * (into_radius + from_radius)) { + // No intersection. + return NULL; + } + + into_intersection_point = from_a; + t1 = 0.0f; } if (collide_cat.is_debug()) { @@ -339,22 +375,28 @@ test_intersection_from_sphere(const CollisionEntry &entry) const { } PT(CollisionEntry) new_entry = new CollisionEntry(entry); + LPoint3f from_center = sphere->get_center() * wrt_mat; + LVector3f surface_normal; - float vec_length = vec.length(); + LVector3f v(into_intersection_point - into_center); + float vec_length = v.length(); if (IS_NEARLY_ZERO(vec_length)) { // If we don't have a collision normal (e.g. the centers are // exactly coincident), then make up an arbitrary normal--any one // is as good as any other. surface_normal.set(1.0, 0.0, 0.0); } else { - surface_normal = vec / vec_length; + surface_normal = v / vec_length; } - LVector3f normal = (has_effective_normal() && sphere->get_respect_effective_normal()) ? get_effective_normal() : surface_normal; + LVector3f eff_normal = (has_effective_normal() && sphere->get_respect_effective_normal()) ? get_effective_normal() : surface_normal; - new_entry->set_surface_normal(normal); + new_entry->set_surface_normal(eff_normal); new_entry->set_surface_point(into_center + surface_normal * into_radius); new_entry->set_interior_point(from_center - surface_normal * from_radius); + new_entry->set_contact_pos(into_intersection_point); + new_entry->set_contact_normal(surface_normal); + new_entry->set_t(t1); return new_entry; } @@ -375,7 +417,7 @@ test_intersection_from_line(const CollisionEntry &entry) const { LVector3f from_direction = line->get_direction() * wrt_mat; double t1, t2; - if (!intersects_line(t1, t2, from_origin, from_direction)) { + if (!intersects_line(t1, t2, from_origin, from_direction, 0.0f)) { // No intersection. return NULL; } @@ -417,7 +459,7 @@ test_intersection_from_ray(const CollisionEntry &entry) const { LVector3f from_direction = ray->get_direction() * wrt_mat; double t1, t2; - if (!intersects_line(t1, t2, from_origin, from_direction)) { + if (!intersects_line(t1, t2, from_origin, from_direction, 0.0f)) { // No intersection. return NULL; } @@ -467,7 +509,7 @@ test_intersection_from_segment(const CollisionEntry &entry) const { LVector3f from_direction = from_b - from_a; double t1, t2; - if (!intersects_line(t1, t2, from_a, from_direction)) { + if (!intersects_line(t1, t2, from_a, from_direction, 0.0f)) { // No intersection. return NULL; } @@ -560,7 +602,8 @@ fill_viz_geom() { //////////////////////////////////////////////////////////////////// bool CollisionSphere:: intersects_line(double &t1, double &t2, - const LPoint3f &from, const LVector3f &delta) const { + const LPoint3f &from, const LVector3f &delta, + float inflate_radius) const { // Solve the equation for the intersection of a line with a sphere // using the quadratic equation. @@ -598,7 +641,8 @@ intersects_line(double &t1, double &t2, LVector3f fc = from - get_center(); double B = 2.0f* dot(delta, fc); double fc_d2 = dot(fc, fc); - double C = fc_d2 - get_radius() * get_radius(); + double radius = get_radius() + inflate_radius; + double C = fc_d2 - radius * radius; double radical = B*B - 4.0*A*C; diff --git a/panda/src/collide/collisionSphere.h b/panda/src/collide/collisionSphere.h index 643670b84b..2b53657364 100644 --- a/panda/src/collide/collisionSphere.h +++ b/panda/src/collide/collisionSphere.h @@ -79,7 +79,8 @@ protected: protected: bool intersects_line(double &t1, double &t2, - const LPoint3f &from, const LVector3f &delta) const; + const LPoint3f &from, const LVector3f &delta, + float inflate_radius) const; Vertexf compute_point(float latitude, float longitude) const; private: diff --git a/panda/src/collide/collisionTube.cxx b/panda/src/collide/collisionTube.cxx index 059733c746..0dc0854cc0 100644 --- a/panda/src/collide/collisionTube.cxx +++ b/panda/src/collide/collisionTube.cxx @@ -804,10 +804,7 @@ set_intersection_point(CollisionEntry *new_entry, point = point * _mat; normal = normal * _mat; - // Also adjust the original point into the tube by the amount of - // extra_radius, which should put it on the surface of the tube if - // our collision was tangential. - LPoint3f orig_point = into_intersection_point - normal * extra_radius; + LVector3f contact_normal(normal); if (has_effective_normal() && new_entry->get_from()->get_respect_effective_normal()) { normal = get_effective_normal(); @@ -815,7 +812,12 @@ set_intersection_point(CollisionEntry *new_entry, new_entry->set_surface_normal(normal); new_entry->set_surface_point(point); - new_entry->set_interior_point(orig_point); + // Also adjust the original point into the tube by the amount of + // extra_radius, which should put it on the surface of the tube if + // our collision was tangential. + new_entry->set_interior_point(into_intersection_point - normal * extra_radius); + new_entry->set_contact_pos(into_intersection_point); + new_entry->set_contact_normal(contact_normal); } ////////////////////////////////////////////////////////////////////