TheCloud/panda3d
1
0
Fork 0
mirror of https://github.com/panda3d/panda3d.git synced 2025-10-03 10:22:45 -04:00
Code Issues Packages Projects Releases Wiki Activity

Fix tendency of CollisionFloorMesh to introduce NaN's into result

Browse Source
This commit is contained in:
David Rose 2011-06-02 18:05:48 +00:00
parent 2d6edaf18b
commit 33886780c4
2 changed files with 55 additions and 47 deletions
Show Stats Download Patch File Download Diff File Expand all files Collapse all files

6
panda/src/collide/collisionEntry.I
View File

@ -131,6 +131,7 @@ get_into_node_path() const {
////////////////////////////////////////////////////////////////////
INLINE void CollisionEntry::
set_t(float t) {
nassertv(!cnan(t));
_t = t;
}
@ -193,6 +194,7 @@ get_respect_prev_transform() const {
////////////////////////////////////////////////////////////////////
INLINE void CollisionEntry::
set_surface_point(const LPoint3f &point) {
nassertv(!point.is_nan());
_surface_point = point;
_flags |= F_has_surface_point;
}
@ -208,6 +210,7 @@ set_surface_point(const LPoint3f &point) {
////////////////////////////////////////////////////////////////////
INLINE void CollisionEntry::
set_surface_normal(const LVector3f &normal) {
nassertv(!normal.is_nan());
_surface_normal = normal;
_flags |= F_has_surface_normal;
}
@ -227,6 +230,7 @@ set_surface_normal(const LVector3f &normal) {
////////////////////////////////////////////////////////////////////
INLINE void CollisionEntry::
set_interior_point(const LPoint3f &point) {
nassertv(!point.is_nan());
_interior_point = point;
_flags |= F_has_interior_point;
}
@ -281,6 +285,7 @@ has_interior_point() const {
////////////////////////////////////////////////////////////////////
INLINE void CollisionEntry::
set_contact_pos(const LPoint3f &pos) {
nassertv(!pos.is_nan());
_contact_pos = pos;
_flags |= F_has_contact_pos;
}
@ -296,6 +301,7 @@ set_contact_pos(const LPoint3f &pos) {
////////////////////////////////////////////////////////////////////
INLINE void CollisionEntry::
set_contact_normal(const LVector3f &normal) {
nassertv(!normal.is_nan());
_contact_normal = normal;
_flags |= F_has_contact_normal;
}

96
panda/src/collide/collisionFloorMesh.cxx
View File

@ -145,55 +145,56 @@ test_intersection_from_ray(const CollisionEntry &entry) const {
DCAST_INTO_R(ray, entry.get_from(), 0);
LPoint3f from_origin = ray->get_origin() * entry.get_wrt_mat();
double fx=from_origin[0];
double fy=from_origin[1];
double fx = from_origin[0];
double fy = from_origin[1];
CollisionFloorMesh::Triangles::const_iterator ti;
for (ti=_triangles.begin();ti< _triangles.end();++ti) {
for (ti = _triangles.begin(); ti < _triangles.end(); ++ti) {
TriangleIndices tri = *ti;
//First do a naive bounding box check on the triangle
if(fx<tri.min_x || fx>=tri.max_x || fy<tri.min_y || fy>=tri.max_y)
if (fx < tri.min_x || fx >= tri.max_x || fy < tri.min_y || fy >= tri.max_y) {
continue;
}
//okay, there's a good chance we'll be colliding
LPoint3f p0=_vertices[tri.p1];
LPoint3f p1=_vertices[tri.p2];
LPoint3f p2=_vertices[tri.p3];
LPoint3f p0 = _vertices[tri.p1];
LPoint3f p1 = _vertices[tri.p2];
LPoint3f p2 = _vertices[tri.p3];
float p0x = p0[0];
float p0y = p0[1];
float e0x,e0y,e1x,e1y,e2x,e2y;
float u,v;
float e0x, e0y, e1x, e1y, e2x, e2y;
float u, v;
e0x = fx - p0x; e0y = fy - p0y;
e1x = p1[0] - p0x; e1y = p1[1] - p0y;
e2x = p2[0] - p0x; e2y = p2[1] - p0y;
if (e1x==0) {
if (e2x == 0) continue;
u = e0x/e2x;
if (u<0 || u>1) continue;
if (e1x == 0.0) {
if (e2x == 0.0) continue;
u = e0x / e2x;
if (u < 0.0 || u > 1.0) continue;
if (e1y == 0) continue;
v = ( e0y - (e2y*u))/e1y;
if (v<0) continue;
v = (e0y - (e2y * u)) / e1y;
if (v < 0.0) continue;
} else {
float d = (e2y * e1x)-(e2x * e1y);
if (d==0) continue;
u = ((e0y * e1x) - (e0x * e1y))/d;
if (u<0 || u>1) continue;
float d = (e2y * e1x) - (e2x * e1y);
if (d == 0.0) continue;
u = ((e0y * e1x) - (e0x * e1y)) / d;
if (u < 0.0 || u > 1.0) continue;
v = (e0x - (e2x * u)) / e1x;
if (v<0) continue;
if (u + v > 1) continue;
if (v < 0.0) continue;
}
if (u + v <= 0.0 || u + v > 1.0) continue;
//we collided!!
float mag = u + v;
float p0z = p0[2];
float uz = (p2[2] - p0z) * mag;
float vz = (p1[2] - p0z) * mag;
float finalz = p0z+vz+(((uz - vz) *u)/(u+v));
float finalz = p0z + vz + (((uz - vz) * u) / (u + v));
PT(CollisionEntry) new_entry = new CollisionEntry(entry);
new_entry->set_surface_normal(LPoint3f(0,0,1));
new_entry->set_surface_point(LPoint3f(fx,fy,finalz));
new_entry->set_surface_normal(LPoint3f(0, 0, 1));
new_entry->set_surface_point(LPoint3f(fx, fy, finalz));
return new_entry;
}
return NULL;
@ -201,9 +202,9 @@ test_intersection_from_ray(const CollisionEntry &entry) const {
////////////////////////////////////////////////////////////////////
// Function: CollisionFloorMesh::test_intersection_from_ray
// Function: CollisionFloorMesh::test_intersection_from_sphere
// Access: Public, Virtual
// Description: must be a vertical Ray!!!
// Description:
////////////////////////////////////////////////////////////////////
PT(CollisionEntry) CollisionFloorMesh::
test_intersection_from_sphere(const CollisionEntry &entry) const {
@ -217,40 +218,41 @@ test_intersection_from_sphere(const CollisionEntry &entry) const {
float fz = float(from_origin[2]);
float rad = sphere->get_radius();
CollisionFloorMesh::Triangles::const_iterator ti;
for (ti=_triangles.begin();ti< _triangles.end();++ti) {
for (ti = _triangles.begin(); ti < _triangles.end(); ++ti) {
TriangleIndices tri = *ti;
//First do a naive bounding box check on the triangle
if(fx<tri.min_x || fx>=tri.max_x || fy<tri.min_y || fy>=tri.max_y)
if (fx < tri.min_x || fx >= tri.max_x || fy < tri.min_y || fy >= tri.max_y) {
continue;
}
//okay, there's a good chance we'll be colliding
LPoint3f p0=_vertices[tri.p1];
LPoint3f p1=_vertices[tri.p2];
LPoint3f p2=_vertices[tri.p3];
LPoint3f p0 = _vertices[tri.p1];
LPoint3f p1 = _vertices[tri.p2];
LPoint3f p2 = _vertices[tri.p3];
float p0x = p0[0];
float p0y = p0[1];
float e0x,e0y,e1x,e1y,e2x,e2y;
float u,v;
float e0x, e0y, e1x, e1y, e2x, e2y;
float u, v;
e0x = fx - p0x; e0y = fy - p0y;
e1x = p1[0] - p0x; e1y = p1[1] - p0y;
e2x = p2[0] - p0x; e2y = p2[1] - p0y;
if (e1x==0) {
if (e2x == 0) continue;
u = e0x/e2x;
if (u<0 || u>1) continue;
if (e1x == 0.0) {
if (e2x == 0.0) continue;
u = e0x / e2x;
if (u < 0.0 || u > 1.0) continue;
if (e1y == 0) continue;
v = ( e0y - (e2y*u))/e1y;
if (v<0) continue;
v = (e0y - (e2y * u)) / e1y;
if (v < 0.0) continue;
} else {
float d = (e2y * e1x)-(e2x * e1y);
if (d==0) continue;
u = ((e0y * e1x) - (e0x * e1y))/d;
if (u<0 || u>1) continue;
float d = (e2y * e1x) - (e2x * e1y);
if (d == 0.0) continue;
u = ((e0y * e1x) - (e0x * e1y)) / d;
if (u < 0.0 || u > 1.0) continue;
v = (e0x - (e2x * u)) / e1x;
if (v<0) continue;
if (u + v > 1) continue;
if (v < 0.0) continue;
}
if (u + v <= 0.0 || u + v > 1.0) continue;
//we collided!!
float mag = u + v;
float p0z = p0[2];
@ -263,8 +265,8 @@ test_intersection_from_sphere(const CollisionEntry &entry) const {
return NULL;
PT(CollisionEntry) new_entry = new CollisionEntry(entry);
new_entry->set_surface_normal(LPoint3f(0,0,1));
new_entry->set_surface_point(LPoint3f(fx,fy,finalz));
new_entry->set_surface_normal(LPoint3f(0, 0, 1));
new_entry->set_surface_point(LPoint3f(fx, fy, finalz));
return new_entry;
}
return NULL;