MATH_BEGIN_NAMESPACE /// Computes the closest point pair on two lines. /** The first line is specified by two points start0 and end0. The second line is specified by two points start1 and end1. The implementation of this function follows http://paulbourke.net/geometry/lineline3d/ . @param v0 The starting point of the first line. @param v10 The direction vector of the first line. This can be unnormalized. @param v2 The starting point of the second line. @param v32 The direction vector of the second line. This can be unnormalized. @param d [out] Receives the normalized distance of the closest point along the first line. @param d2 [out] Receives the normalized distance of the closest point along the second line. @return Returns the closest point on line start0<->end0 to the second line. @note This is a low-level utility function. You probably want to use ClosestPoint() or Distance() instead. @see ClosestPoint(), Distance(). */ void Line::ClosestPointLineLine(const vec &v0, const vec &v10, const vec &v2, const vec &v32, float &d, float &d2) { assume(!v10.IsZero()); assume(!v32.IsZero()); vec v02 = v0 - v2; float d0232 = v02.Dot(v32); float d3210 = v32.Dot(v10); float d3232 = v32.Dot(v32); assume(d3232 != 0.f); // Don't call with a zero direction vector. float d0210 = v02.Dot(v10); float d1010 = v10.Dot(v10); float denom = d1010*d3232 - d3210*d3210; if (denom != 0.f) d = (d0232*d3210 - d0210*d3232) / denom; else d = 0.f; d2 = (d0232 + d * d3210) / d3232; }
vec Plane::Mirror(const vec &point) const { #ifdef MATH_ASSERT_CORRECTNESS float signedDistance = SignedDistance(point); #endif assume2(normal.IsNormalized(), normal.SerializeToCodeString(), normal.Length()); vec reflected = point - 2.f * (point.Dot(normal) - d) * normal; mathassert(EqualAbs(signedDistance, -SignedDistance(reflected), 1e-2f)); mathassert(reflected.Equals(MirrorMatrix().MulPos(point))); return reflected; }
void Plane::Set(const vec &point, const vec &normal_) { normal = normal_; assume2(normal.IsNormalized(), normal.SerializeToCodeString(), normal.Length()); d = point.Dot(normal); #ifdef MATH_ASSERT_CORRECTNESS assert1(EqualAbs(SignedDistance(point), 0.f, 0.01f), SignedDistance(point)); assert1(EqualAbs(SignedDistance(point + normal_), 1.f, 0.01f), SignedDistance(point + normal_)); #endif }
void AABB::ProjectToAxis(const vec &axis, float &dMin, float &dMax) const { vec c = CenterPoint(); vec e = HalfDiagonal(); // Compute the projection interval radius of the AABB onto L(t) = aabb.center + t * plane.normal; float r = e[0]*Abs(axis[0]) + e[1]*Abs(axis[1]) + e[2]*Abs(axis[2]); // Compute the distance of the box center from plane. float s = axis.Dot(c); dMin = s - r; dMax = s + r; if (dMin > dMax) Swap(dMin, dMax); }
void AABB::ProjectToAxis(const vec &axis, float &dMin, float &dMax) const { vec c = (minPoint + maxPoint) * 0.5f; vec e = maxPoint - c; #if defined(MATH_AUTOMATIC_SSE) && defined(MATH_SIMD) vec absAxis = axis.Abs(); float r = Abs(e.Dot(absAxis)); #else // Compute the projection interval radius of the AABB onto L(t) = aabb.center + t * plane.normal; float r = Abs(e[0]*Abs(axis[0]) + e[1]*Abs(axis[1]) + e[2]*Abs(axis[2])); #endif // Compute the distance of the box center from plane. float s = axis.Dot(c); dMin = s - r; dMax = s + r; }