contact_pos for collision polys, spheres and tubes, improvements to fluidpusher

This commit is contained in:
Darren Ranalli 2007-09-03 07:39:30 +00:00
parent 7055683eb5
commit 42795391de
9 changed files with 134 additions and 86 deletions

View File

@ -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;
}
////////////////////////////////////////////////////////////////////

View File

@ -41,7 +41,7 @@ CollisionEntry(const CollisionEntry &copy) :
_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 &copy) {
_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()) {

View File

@ -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:

View File

@ -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);

View File

@ -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;

View File

@ -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);

View File

@ -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;

View File

@ -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:

View File

@ -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);
}
////////////////////////////////////////////////////////////////////