diff --git a/panda/src/collide/collisionHandlerPusher.cxx b/panda/src/collide/collisionHandlerPusher.cxx index 94e069f2f2..314fc0f511 100644 --- a/panda/src/collide/collisionHandlerPusher.cxx +++ b/panda/src/collide/collisionHandlerPusher.cxx @@ -172,28 +172,19 @@ handle_entries() { } } - LMatrix4f mat; - def.get_mat(mat); - - // def.set_mat(LMatrix4f::translate_mat(net_shove) * mat); - - mat(3,0) += net_shove[0]; - mat(3,1) += net_shove[1]; - - if (!_horizontal) { - mat(3,2) += net_shove[2]; + if (_horizontal) { + net_shove[2] = 0.0; } - def.set_mat(mat); - if (collide_cat.is_debug()) { - if (_horizontal) - net_shove[2] = 0.0; - collide_cat.debug() << "Net shove on " << *from_node << " is: " << net_shove << "\n"; } + + LMatrix4f mat; + def.get_mat(mat); + def.set_mat(LMatrix4f::translate_mat(net_shove) * mat); } } } diff --git a/panda/src/collide/collisionPlane.cxx b/panda/src/collide/collisionPlane.cxx index 28b1532687..ffb51f90c1 100644 --- a/panda/src/collide/collisionPlane.cxx +++ b/panda/src/collide/collisionPlane.cxx @@ -125,17 +125,11 @@ test_intersection_from_sphere(CollisionHandler *record, DCAST_INTO_R(sphere, entry.get_from(), 0); LPoint3f from_center = sphere->get_center() * entry.get_wrt_space(); + LVector3f from_radius_v = + LVector3f(sphere->get_radius(), 0.0f, 0.0f) * entry.get_wrt_space(); + float from_radius = length(from_radius_v); + float dist = dist_to_plane(from_center); -// LVector3f from_radius_v = -// LVector3f(sphere->get_radius(), 0.0f, 0.0f) * entry.get_wrt_space(); -// float from_radius = length(from_radius_v); - - const LMatrix4f *pMat = &entry.get_wrt_space(); - float from_radius = sphere->get_radius() * - sqrtf((*pMat)(0,0)*(*pMat)(0,0) + - (*pMat)(0,1)*(*pMat)(0,1) + - (*pMat)(0,2)*(*pMat)(0,2)); - if (dist > from_radius) { // No intersection. return 0; diff --git a/panda/src/collide/collisionPolygon.cxx b/panda/src/collide/collisionPolygon.cxx index dfb01fe34f..b3f45dde22 100644 --- a/panda/src/collide/collisionPolygon.cxx +++ b/panda/src/collide/collisionPolygon.cxx @@ -239,20 +239,12 @@ test_intersection_from_sphere(CollisionHandler *record, DCAST_INTO_R(sphere, entry.get_from(), 0); LPoint3f from_center = sphere->get_center() * entry.get_wrt_space(); + LVector3f from_radius_v = + LVector3f(sphere->get_radius(), 0.0f, 0.0f) * entry.get_wrt_space(); + float from_radius = length(from_radius_v); + float dist = dist_to_plane(from_center); - -// LVector3f from_radius_v = -// LVector3f(sphere->get_radius(), 0.0f, 0.0f) * entry.get_wrt_space(); -// float from_radius = length(from_radius_v); - - const LMatrix4f *pMat = &entry.get_wrt_space(); - float from_radius = sphere->get_radius() * - sqrtf((*pMat)(0,0)*(*pMat)(0,0) + - (*pMat)(0,1)*(*pMat)(0,1) + - (*pMat)(0,2)*(*pMat)(0,2)); - -// if (dist > from_radius || dist < -from_radius) { - if (fabs(dist) > from_radius) { + if (dist > from_radius || dist < -from_radius) { // No intersection. return 0; } @@ -265,9 +257,7 @@ test_intersection_from_sphere(CollisionHandler *record, bool really_intersects = get_plane().intersects_line(plane_point, from_center, from_center + get_normal()); -#ifdef _DEBUG nassertr(really_intersects, 0); -#endif LPoint2f p = to_2d(plane_point); diff --git a/panda/src/collide/collisionSphere.cxx b/panda/src/collide/collisionSphere.cxx index f3fb01cc10..562ef25801 100644 --- a/panda/src/collide/collisionSphere.cxx +++ b/panda/src/collide/collisionSphere.cxx @@ -123,22 +123,17 @@ test_intersection_from_sphere(CollisionHandler *record, const CollisionSphere *sphere; DCAST_INTO_R(sphere, entry.get_from(), 0); - //from_radius_v = LVector3f(sphere->get_radius(), 0.0, 0.0) * entry.get_wrt_space(); - //float from_radius = length(from_radius_v); - - const LMatrix4f *pMat = &entry.get_wrt_space(); - float from_radius = sphere->get_radius() * - sqrtf((*pMat)(0,0)*(*pMat)(0,0) + - (*pMat)(0,1)*(*pMat)(0,1) + - (*pMat)(0,2)*(*pMat)(0,2)); - LPoint3f from_center = sphere->get_center() * entry.get_wrt_space(); - LVector3f vec = from_center - _center; + LVector3f from_radius_v = + LVector3f(sphere->get_radius(), 0.0, 0.0) * entry.get_wrt_space(); + float from_radius = length(from_radius_v); + LPoint3f into_center = _center; + float into_radius = _radius; + + LVector3f vec = from_center - into_center; float dist2 = dot(vec, vec); - float total_radius = _radius + from_radius; - - if (dist2 > total_radius * total_radius) { + if (dist2 > (into_radius + from_radius) * (into_radius + from_radius)) { // No intersection. return 0; } @@ -153,7 +148,7 @@ test_intersection_from_sphere(CollisionHandler *record, float dist = sqrtf(dist2); LVector3f into_normal = normalize(vec); LPoint3f into_intersection_point = into_normal * (dist - from_radius); - float into_depth = total_radius - dist; + float into_depth = into_radius + from_radius - dist; new_entry->set_into_surface_normal(into_normal * entry.get_inv_wrt_space()); new_entry->set_into_intersection_point(into_intersection_point * entry.get_inv_wrt_space());