diff --git a/panda/src/mathutil/plane_src.I b/panda/src/mathutil/plane_src.I index f357b34e60..7ea0acc828 100644 --- a/panda/src/mathutil/plane_src.I +++ b/panda/src/mathutil/plane_src.I @@ -137,6 +137,37 @@ dist_to_plane(const FLOATNAME(LPoint3) &point) const { return (_v(0) * point[0] + _v(1) * point[1] + _v(2) * point[2] + _v(3)); } +/** + * Normalizes the plane in place. Returns true if the plane was normalized, + * false if the plane had a zero-length normal vector. + */ +INLINE_MATHUTIL bool FLOATNAME(LPlane):: +normalize() { + FLOATTYPE l2 = get_normal().length_squared(); + if (l2 == (FLOATTYPE)0.0f) { + return false; + + } else if (!IS_THRESHOLD_EQUAL(l2, 1.0f, NEARLY_ZERO(FLOATTYPE) * NEARLY_ZERO(FLOATTYPE))) { + (*this) /= csqrt(l2); + } + + return true; +} + +/** + * Normalizes the plane and returns the normalized plane as a copy. If the + * plane's normal was a zero-length vector, the same plane is returned. + */ +INLINE_MATHUTIL FLOATNAME(LPlane) FLOATNAME(LPlane):: +normalized() const { + FLOATTYPE l2 = get_normal().length_squared(); + if (l2 != (FLOATTYPE)0.0f) { + return (*this) / csqrt(l2); + } else { + return (*this); + } +} + /** * Returns the point within the plane nearest to the indicated point in space. */ diff --git a/panda/src/mathutil/plane_src.h b/panda/src/mathutil/plane_src.h index c3301aa641..c1dffc4d5d 100644 --- a/panda/src/mathutil/plane_src.h +++ b/panda/src/mathutil/plane_src.h @@ -39,6 +39,9 @@ PUBLISHED: FLOATNAME(LPoint3) get_point() const; INLINE_MATHUTIL FLOATTYPE dist_to_plane(const FLOATNAME(LPoint3) &point) const; + + INLINE_MATHUTIL bool normalize(); + INLINE_MATHUTIL FLOATNAME(LPlane) normalized() const; INLINE_MATHUTIL FLOATNAME(LPoint3) project(const FLOATNAME(LPoint3) &point) const; INLINE_MATHUTIL void flip();