float Line::Distance(const LineSegment &other, float &d, float &d2) const { vec c = ClosestPoint(other, d, d2); mathassert(d2 >= 0.f); mathassert(d2 <= 1.f); return c.Distance(other.GetPoint(d2)); }
bool Polygon::Contains(const float3 &worldSpacePoint, float polygonThickness) const { // Implementation based on the description from http://erich.realtimerendering.com/ptinpoly/ if (p.size() < 3) return false; if (PlaneCCW().Distance(worldSpacePoint) > polygonThickness) return false; int numIntersections = 0; float3 basisU = BasisU(); float3 basisV = BasisV(); mathassert(basisU.IsNormalized()); mathassert(basisV.IsNormalized()); mathassert(basisU.IsPerpendicular(basisV)); mathassert(basisU.IsPerpendicular(PlaneCCW().normal)); mathassert(basisV.IsPerpendicular(PlaneCCW().normal)); float2 localSpacePoint = float2(Dot(worldSpacePoint, basisU), Dot(worldSpacePoint, basisV)); const float epsilon = 1e-4f; float2 p0 = float2(Dot(p[p.size()-1], basisU), Dot(p[p.size()-1], basisV)) - localSpacePoint; if (Abs(p0.y) < epsilon) p0.y = -epsilon; // Robustness check - if the ray (0,0) -> (+inf, 0) would pass through a vertex, move the vertex slightly. for(int i = 0; i < (int)p.size(); ++i) { float2 p1 = float2(Dot(p[i], basisU), Dot(p[i], basisV)) - localSpacePoint; if (Abs(p1.y) < epsilon) p0.y = -epsilon; // Robustness check - if the ray (0,0) -> (+inf, 0) would pass through a vertex, move the vertex slightly. if (p0.y * p1.y < 0.f) { if (p0.x > 1e-3f && p1.x > 1e-3f) ++numIntersections; else { // P = p0 + t*(p1-p0) == (x,0) // p0.x + t*(p1.x-p0.x) == x // p0.y + t*(p1.y-p0.y) == 0 // t == -p0.y / (p1.y - p0.y) // Test whether the lines (0,0) -> (+inf,0) and p0 -> p1 intersect at a positive X-coordinate. float2 d = p1 - p0; if (Abs(d.y) > 1e-5f) { float t = -p0.y / d.y; float x = p0.x + t * d.x; if (t >= 0.f && t <= 1.f && x > 1e-3f) ++numIntersections; } } } p0 = p1; } return numIntersections % 2 == 1; }
float Line::Distance(const LineSegment &other, float *d, float *d2) const { float u2; vec c = ClosestPoint(other, d, &u2); if (d2) *d2 = u2; mathassert(u2 >= 0.f); mathassert(u2 <= 1.f); return c.Distance(other.GetPoint(u2)); }
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; }
Sphere Sphere::OptimalEnclosingSphere(const vec &a, const vec &b, const vec &c, const vec &d, const vec &e, int &redundantPoint) { Sphere s = OptimalEnclosingSphere(b,c,d,e); if (s.Contains(a, sEpsilon)) { redundantPoint = 0; return s; } s = OptimalEnclosingSphere(a,c,d,e); if (s.Contains(b, sEpsilon)) { redundantPoint = 1; return s; } s = OptimalEnclosingSphere(a,b,d,e); if (s.Contains(c, sEpsilon)) { redundantPoint = 2; return s; } s = OptimalEnclosingSphere(a,b,c,e); if (s.Contains(d, sEpsilon)) { redundantPoint = 3; return s; } s = OptimalEnclosingSphere(a,b,c,d); mathassert(s.Contains(e, sEpsilon)); redundantPoint = 4; return s; }
Sphere Sphere::OptimalEnclosingSphere(const vec &a, const vec &b) { Sphere s; s.pos = (a + b) * 0.5f; s.r = (b - s.pos).Length(); assume(s.pos.IsFinite()); assume(s.r >= 0.f); // Allow floating point inconsistency and expand the radius by a small epsilon so that the containment tests // really contain the points (note that the points must be sufficiently near enough to the origin) s.r += sEpsilon; mathassert(s.Contains(a)); mathassert(s.Contains(b)); return s; }
bool float3x4::InverseColOrthogonal() { #if defined(MATH_AUTOMATIC_SSE) && defined(MATH_SSE) mat3x4_inverse_colorthogonal(row, row); #else assume(IsColOrthogonal()); float s1 = float3(v[0][0], v[1][0], v[2][0]).LengthSq(); float s2 = float3(v[0][1], v[1][1], v[2][1]).LengthSq(); float s3 = float3(v[0][2], v[1][2], v[2][2]).LengthSq(); if (s1 < 1e-8f || s2 < 1e-8f || s3 < 1e-8f) return false; s1 = 1.f / s1; s2 = 1.f / s2; s3 = 1.f / s3; Swap(v[0][1], v[1][0]); Swap(v[0][2], v[2][0]); Swap(v[1][2], v[2][1]); v[0][0] *= s1; v[0][1] *= s1; v[0][2] *= s1; v[1][0] *= s2; v[1][1] *= s2; v[1][2] *= s2; v[2][0] *= s3; v[2][1] *= s3; v[2][2] *= s3; SetTranslatePart(TransformDir(-v[0][3], -v[1][3], -v[2][3])); mathassert(IsRowOrthogonal()); #endif return true; }
float3x4 operator *(const TranslateOp &lhs, const float3x4 &rhs) { float3x4 r = rhs; r.SetTranslatePart(r.TranslatePart() + DIR_TO_FLOAT3(lhs.Offset())); // Our optimized form of multiplication must be the same as this. mathassert(r.Equals((float3x4)lhs * rhs)); return r; }
float4x4 operator *(const TranslateOp &lhs, const float4x4 &rhs) { float4x4 r = rhs; r.SetTranslatePart(r.TranslatePart() + lhs.Offset()); // Our optimized form of multiplication must be the same as this. mathassert(r.Equals(lhs.ToFloat4x4() * rhs)); return r; }
float3x4 operator *(const float3x4 &lhs, const TranslateOp &rhs) { float3x4 r = lhs; r.SetTranslatePart(lhs.TransformPos(rhs.Offset())); // Our optimized form of multiplication must be the same as this. mathassert(r.Equals(lhs * (float3x4)rhs)); return r; }
float3x4 operator *(const ScaleOp &lhs, const float3x4 &rhs) { float3x4 ret; ret[0][0] = rhs[0][0] * lhs.scale.x; ret[0][1] = rhs[0][1] * lhs.scale.x; ret[0][2] = rhs[0][2] * lhs.scale.x; ret[0][3] = rhs[0][3] * lhs.scale.x; ret[1][0] = rhs[1][0] * lhs.scale.y; ret[1][1] = rhs[1][1] * lhs.scale.y; ret[1][2] = rhs[1][2] * lhs.scale.y; ret[1][3] = rhs[1][3] * lhs.scale.y; ret[2][0] = rhs[2][0] * lhs.scale.z; ret[2][1] = rhs[2][1] * lhs.scale.z; ret[2][2] = rhs[2][2] * lhs.scale.z; ret[2][3] = rhs[2][3] * lhs.scale.z; mathassert(ret.Equals(lhs.ToFloat3x4() * rhs)); return ret; }
float3x4 operator *(const float3x4 &lhs, const ScaleOp &rhs) { float3x4 ret; ret[0][0] = lhs[0][0] * rhs.x; ret[0][1] = lhs[0][1] * rhs.y; ret[0][2] = lhs[0][2] * rhs.z; ret[0][3] = lhs[0][3]; ret[1][0] = lhs[1][0] * rhs.x; ret[1][1] = lhs[1][1] * rhs.y; ret[1][2] = lhs[1][2] * rhs.z; ret[1][3] = lhs[1][3]; ret[2][0] = lhs[2][0] * rhs.x; ret[2][1] = lhs[2][1] * rhs.y; ret[2][2] = lhs[2][2] * rhs.z; ret[2][3] = lhs[2][3]; mathassert(ret.Equals(lhs * rhs.ToFloat3x4())); return ret; }
float3x4 operator *(const TranslateOp &lhs, const ScaleOp &rhs) { float3x4 ret; ret[0][0] = rhs.x; ret[0][1] = 0; ret[0][2] = 0; ret[0][3] = lhs.x; ret[1][0] = 0; ret[1][1] = rhs.y; ret[1][2] = 0; ret[1][3] = lhs.y; ret[2][0] = 0; ret[2][1] = 0; ret[2][2] = rhs.z; ret[2][3] = lhs.z; mathassert(ret.Equals(lhs.ToFloat3x4() * rhs)); return ret; }
bool float3x4::IsInvertible(float epsilon) const { float d = Determinant(); bool isSingular = EqualAbs(d, 0.f, epsilon); #ifdef MATH_ASSERT_CORRECTNESS float3x3 temp = Float3x3Part(); mathassert(temp.Inverse(epsilon) != isSingular); // IsInvertible() and Inverse() must match! #endif return !isSingular; }
float3x3 operator *(const ScaleOp &lhs, const float3x3 &rhs) { float3x3 ret = rhs; ret.ScaleRow(0, lhs.x); ret.ScaleRow(1, lhs.y); ret.ScaleRow(2, lhs.z); // Our optimized form of multiplication must be the same as this. mathassert(ret.Equals((float3x3)lhs * rhs)); return ret; }
float3x3 operator *(const float3x3 &lhs, const ScaleOp &rhs) { float3x3 ret = lhs; ret.ScaleCol(0, rhs.x); ret.ScaleCol(1, rhs.y); ret.ScaleCol(2, rhs.z); // Our optimized form of multiplication must be the same as this. mathassert(ret.Equals(lhs * (float3x3)rhs)); return ret; }
void Quat::Set(const float3x4 &m) { assume(m.IsColOrthogonal()); assume(m.HasUnitaryScale()); assume(!m.HasNegativeScale()); SetQuatFrom(*this, m); #ifdef MATH_ASSERT_CORRECTNESS // Test that the conversion float3x3->Quat->float3x3 is correct. mathassert(this->ToFloat3x3().Equals(m.Float3x3Part(), 0.01f)); #endif }
float4x4 operator *(const ScaleOp &lhs, const float4x4 &rhs) { float4x4 ret; #if defined(MATH_AUTOMATIC_SSE) && defined(MATH_SIMD) simd4f x = xxxx_ps(lhs.scale.v); simd4f y = yyyy_ps(lhs.scale.v); simd4f z = zzzz_ps(lhs.scale.v); ret.row[0] = mul_ps(rhs.row[0], x); ret.row[1] = mul_ps(rhs.row[1], y); ret.row[2] = mul_ps(rhs.row[2], z); ret.row[3] = rhs.row[3]; #else ret[0][0] = rhs[0][0] * lhs.scale.x; ret[0][1] = rhs[0][1] * lhs.scale.x; ret[0][2] = rhs[0][2] * lhs.scale.x; ret[0][3] = rhs[0][3] * lhs.scale.x; ret[1][0] = rhs[1][0] * lhs.scale.y; ret[1][1] = rhs[1][1] * lhs.scale.y; ret[1][2] = rhs[1][2] * lhs.scale.y; ret[1][3] = rhs[1][3] * lhs.scale.y; ret[2][0] = rhs[2][0] * lhs.scale.z; ret[2][1] = rhs[2][1] * lhs.scale.z; ret[2][2] = rhs[2][2] * lhs.scale.z; ret[2][3] = rhs[2][3] * lhs.scale.z; ret[3][0] = rhs[3][0]; ret[3][1] = rhs[3][1]; ret[3][2] = rhs[3][2]; ret[3][3] = rhs[3][3]; #endif mathassert(ret.Equals(lhs.ToFloat4x4() * rhs)); return ret; }
float3 Polygon::PointOnEdge(float normalizedDistance) const { if (p.empty()) return float3::nan; if (p.size() < 2) return p[0]; normalizedDistance = Frac(normalizedDistance); // Take modulo 1 so we have the range [0,1[. float perimeter = Perimeter(); float d = normalizedDistance * perimeter; for(int i = 0; i < NumVertices(); ++i) { LineSegment edge = Edge(i); float len = edge.Length(); assume(len != 0.f && "Degenerate Polygon detected!"); if (d <= len) return edge.GetPoint(d / len); d -= len; } mathassert(false && "Polygon::PointOnEdge reached end of loop which shouldn't!"); return p[0]; }
void Sphere::Enclose(const vec &point, float epsilon) { vec d = point - pos; float dist2 = d.LengthSq(); if (dist2 + epsilon > r*r) { #ifdef MATH_ASSERT_CORRECTNESS Sphere copy = *this; #endif float dist = Sqrt(dist2); float halfDist = (dist - r) * 0.5f; // Nudge this Sphere towards the target point. Add half the missing distance to radius, // and the other half to position. This gives a tighter enclosure, instead of if // the whole missing distance were just added to radius. pos += d * halfDist / dist; r += halfDist + 1e-4f; // Use a fixed epsilon deliberately, the param is a squared epsilon, so different order of magnitude. #ifdef MATH_ASSERT_CORRECTNESS mathassert(this->Contains(copy, epsilon)); #endif } assume(this->Contains(point)); }
void OBB::Enclose(const vec &point) { vec p = point - pos; for(int i = 0; i < 3; ++i) { assert(EqualAbs(axis[i].Length(), 1.f)); float dist = p.Dot(axis[i]); float distanceFromOBB = Abs(dist) - r[i]; if (distanceFromOBB > 0.f) { r[i] += distanceFromOBB * 0.5f; if (dist > 0.f) ///\todo Optimize out this comparison! pos += axis[i] * distanceFromOBB * 0.5f; else pos -= axis[i] * distanceFromOBB * 0.5f; p = point-pos; ///\todo Can we omit this? (redundant since axis[i] are orthonormal?) mathassert(EqualAbs(Abs(p.Dot(axis[i])), r[i], 1e-1f)); } } // Should now contain the point. assume(Distance(point) <= 1e-3f); }
Polyhedron Polyhedron::ConvexHull(const float3 *pointArray, int numPoints) { ///\todo Check input ptr and size! std::set<int> extremes; const float3 dirs[] = { float3(1,0,0), float3(0,1,0), float3(0,0,1), float3(1,1,0), float3(1,0,1), float3(0,1,1), float3(1,1,1) }; for(size_t i = 0; i < ARRAY_LENGTH(dirs); ++i) { int idx1, idx2; OBB::ExtremePointsAlongDirection(dirs[i], pointArray, numPoints, idx1, idx2); extremes.insert(idx1); extremes.insert(idx2); } Polyhedron p; assume(extremes.size() >= 4); ///\todo Fix this case! int i = 0; std::set<int>::iterator iter = extremes.begin(); for(; iter != extremes.end() && i < 4; ++iter, ++i) p.v.push_back(pointArray[*iter]); Face f; f.v.resize(3); f.v[0] = 0; f.v[1] = 1; f.v[2] = 2; p.f.push_back(f); f.v[0] = 0; f.v[1] = 1; f.v[2] = 3; p.f.push_back(f); f.v[0] = 0; f.v[1] = 2; f.v[2] = 3; p.f.push_back(f); f.v[0] = 1; f.v[1] = 2; f.v[2] = 3; p.f.push_back(f); p.OrientNormalsOutsideConvex(); // Ensure that the winding order of the generated tetrahedron is correct for each face. // assert(p.IsClosed()); //assert(p.IsConvex()); assert(p.FaceIndicesValid()); assert(p.EulerFormulaHolds()); // assert(p.FacesAreNondegeneratePlanar()); CHullHelp hull; for(int j = 0; j < (int)p.f.size(); ++j) hull.livePlanes.push_back(j); // For better performance, merge the remaining extreme points first. for(; iter != extremes.end(); ++iter) { p.MergeConvex(pointArray[*iter]); mathassert(p.FaceIndicesValid()); // mathassert(p.IsClosed()); // mathassert(p.FacesAreNondegeneratePlanar()); // mathassert(p.IsConvex()); } // Merge all the rest of the points. for(int j = 0; j < numPoints; ++j) { if (p.f.size() > 5000 && (j & 255) == 0) printf("Mergeconvex %d/%d, #vertices %d, #faces %d\n", j, numPoints, (int)p.v.size(), (int)p.f.size()); p.MergeConvex(pointArray[i]); mathassert(p.FaceIndicesValid()); // mathassert(p.IsClosed()); // mathassert(p.FacesAreNondegeneratePlanar()); //mathassert(p.IsConvex()); // if (p.f.size() > 5000) // break; } return p; }
bool Polyhedron::FaceContains(int faceIndex, const float3 &worldSpacePoint, float polygonThickness) const { // N.B. This implementation is a duplicate of Polygon::Contains, but adapted to avoid dynamic memory allocation // related to converting the face of a Polyhedron to a Polygon object. // Implementation based on the description from http://erich.realtimerendering.com/ptinpoly/ const Face &face = f[faceIndex]; const std::vector<int> &vertices = face.v; if (vertices.size() < 3) return false; Plane p = FacePlane(faceIndex); if (FacePlane(faceIndex).Distance(worldSpacePoint) > polygonThickness) return false; int numIntersections = 0; float3 basisU = v[vertices[1]] - v[vertices[0]]; basisU.Normalize(); float3 basisV = Cross(p.normal, basisU).Normalized(); mathassert(basisU.IsNormalized()); mathassert(basisV.IsNormalized()); mathassert(basisU.IsPerpendicular(basisV)); mathassert(basisU.IsPerpendicular(p.normal)); mathassert(basisV.IsPerpendicular(p.normal)); float2 localSpacePoint = float2(Dot(worldSpacePoint, basisU), Dot(worldSpacePoint, basisV)); const float epsilon = 1e-4f; float2 p0 = float2(Dot(v[vertices.back()], basisU), Dot(v[vertices.back()], basisV)) - localSpacePoint; if (Abs(p0.y) < epsilon) p0.y = -epsilon; // Robustness check - if the ray (0,0) -> (+inf, 0) would pass through a vertex, move the vertex slightly. for(size_t i = 0; i < vertices.size(); ++i) { float2 p1 = float2(Dot(v[vertices[i]], basisU), Dot(v[vertices[i]], basisV)) - localSpacePoint; if (Abs(p1.y) < epsilon) p0.y = -epsilon; // Robustness check - if the ray (0,0) -> (+inf, 0) would pass through a vertex, move the vertex slightly. if (p0.y * p1.y < 0.f) { if (p0.x > 1e-3f && p1.x > 1e-3f) ++numIntersections; else { // P = p0 + t*(p1-p0) == (x,0) // p0.x + t*(p1.x-p0.x) == x // p0.y + t*(p1.y-p0.y) == 0 // t == -p0.y / (p1.y - p0.y) // Test whether the lines (0,0) -> (+inf,0) and p0 -> p1 intersect at a positive X-coordinate. float2 d = p1 - p0; if (Abs(d.y) > 1e-5f) { float t = -p0.y / d.y; float x = p0.x + t * d.x; if (t >= 0.f && t <= 1.f && x > 1e-6f) ++numIntersections; } } } p0 = p1; } return numIntersections % 2 == 1; }
///\todo Enable this codepath. This if rom Geometric Tools for Computer Graphics, /// but the algorithm in the book is broken and does not take into account the /// direction of the gradient to determine the proper region of intersection. /// Instead using a slower code path above. /// [groupSyntax] float3 Triangle::ClosestPoint(const LineSegment &lineSegment, float3 *otherPt) const { float3 e0 = b - a; float3 e1 = c - a; float3 v_p = a - lineSegment.a; float3 d = lineSegment.b - lineSegment.a; // Q(u,v) = a + u*e0 + v*e1 // L(t) = ls.a + t*d // Minimize the distance |Q(u,v) - L(t)|^2 under u >= 0, v >= 0, u+v <= 1, t >= 0, t <= 1. float v_p_dot_e0 = Dot(v_p, e0); float v_p_dot_e1 = Dot(v_p, e1); float v_p_dot_d = Dot(v_p, d); float3x3 m; m[0][0] = Dot(e0, e0); m[0][1] = Dot(e0, e1); m[0][2] = -Dot(e0, d); m[1][0] = m[0][1]; m[1][1] = Dot(e1, e1); m[1][2] = -Dot(e1, d); m[2][0] = m[0][2]; m[2][1] = m[1][2]; m[2][2] = Dot(d, d); float3 B(-v_p_dot_e0, -v_p_dot_e1, v_p_dot_d); float3 uvt; bool success = m.SolveAxb(B, uvt); if (!success) { float t1, t2, t3; float s1, s2, s3; LineSegment e1 = Edge(0); LineSegment e2 = Edge(1); LineSegment e3 = Edge(2); float d1 = e1.Distance(lineSegment, &t1, &s1); float d2 = e2.Distance(lineSegment, &t2, &s2); float d3 = e3.Distance(lineSegment, &t3, &s3); if (d1 < d2 && d1 < d3) { if (otherPt) *otherPt = lineSegment.GetPoint(s1); return e1.GetPoint(t1); } else if (d2 < d3) { if (otherPt) *otherPt = lineSegment.GetPoint(s2); return e2.GetPoint(t2); } else { if (otherPt) *otherPt = lineSegment.GetPoint(s3); return e3.GetPoint(t3); } } if (uvt.x < 0.f) { // Clamp to u == 0 and solve again. float m_00 = m[2][2]; float m_01 = -m[1][2]; float m_10 = -m[2][1]; float m_11 = m[1][1]; float det = m_00 * m_11 - m_01 * m_10; float v = m_00 * B[1] + m_01 * B[2]; float t = m_10 * B[1] + m_11 * B[2]; v /= det; t /= det; if (v < 0.f) { // Clamp to v == 0 and solve for t. t = B[2] / m[2][2]; t = Clamp01(t); // The solution for t must also be in the range [0,1]. // The solution is (u,v,t)=(0,0,t). if (otherPt) *otherPt = lineSegment.GetPoint(t); return a; } else if (v > 1.f) { // Clamp to v == 1 and solve for t. t = (B[2] - m[2][1]) / m[2][2]; t = Clamp01(t); // The solution is (u,v,t)=(0,1,t). if (otherPt) *otherPt = lineSegment.GetPoint(t); return c; // == a + v*e1 } else if (t < 0.f) { // Clamp to t == 0 and solve for v. v = B[1] / m[1][1]; // mathassert(EqualAbs(v, Clamp01(v))); v = Clamp01(v); // The solution for v must also be in the range [0,1]. TODO: Is this guaranteed by the above? // The solution is (u,v,t)=(0,v,0). if (otherPt) *otherPt = lineSegment.a; return a + v * e1; } else if (t > 1.f) { // Clamp to t == 1 and solve for v. v = (B[1] - m[1][2]) / m[1][1]; // mathassert(EqualAbs(v, Clamp01(v))); v = Clamp01(v); // The solution for v must also be in the range [0,1]. TODO: Is this guaranteed by the above? // The solution is (u,v,t)=(0,v,1). if (otherPt) *otherPt = lineSegment.b; return a + v * e1; } else { // The solution is (u,v,t)=(0,v,t). if (otherPt) *otherPt = lineSegment.GetPoint(t); return a + v * e1; } } else if (uvt.y < 0.f) { // Clamp to v == 0 and solve again. float m_00 = m[2][2]; float m_01 = -m[0][2]; float m_10 = -m[2][0]; float m_11 = m[0][0]; float det = m_00 * m_11 - m_01 * m_10; float u = m_00 * B[0] + m_01 * B[2]; float t = m_10 * B[0] + m_11 * B[2]; u /= det; t /= det; if (u < 0.f) { // Clamp to u == 0 and solve for t. t = B[2] / m[2][2]; t = Clamp01(t); // The solution for t must also be in the range [0,1]. // The solution is (u,v,t)=(0,0,t). if (otherPt) *otherPt = lineSegment.GetPoint(t); return a; } else if (u > 1.f) { // Clamp to u == 1 and solve for t. t = (B[2] - m[2][0]) / m[2][2]; t = Clamp01(t); // The solution for t must also be in the range [0,1]. // The solution is (u,v,t)=(1,0,t). if (otherPt) *otherPt = lineSegment.GetPoint(t); return b; } else if (t < 0.f) { // Clamp to t == 0 and solve for u. u = B[0] / m[0][0]; // mathassert(EqualAbs(u, Clamp01(u))); u = Clamp01(u); // The solution for u must also be in the range [0,1]. if (otherPt) *otherPt = lineSegment.a; return a + u * e0; } else if (t > 1.f) { // Clamp to t == 1 and solve for u. u = (B[0] - m[0][2]) / m[0][0]; // mathassert(EqualAbs(u, Clamp01(u))); u = Clamp01(u); // The solution for u must also be in the range [0,1]. if (otherPt) *otherPt = lineSegment.b; return a + u * e0; } else { // The solution is (u, 0, t). if (otherPt) *otherPt = lineSegment.GetPoint(t); return a + u * e0; } } else if (uvt.z < 0.f) { if (otherPt) *otherPt = lineSegment.a; // Clamp to t == 0 and solve again. float m_00 = m[1][1]; float m_01 = -m[0][1]; float m_10 = -m[1][0]; float m_11 = m[0][0]; float det = m_00 * m_11 - m_01 * m_10; float u = m_00 * B[0] + m_01 * B[1]; float v = m_10 * B[0] + m_11 * B[1]; u /= det; v /= det; if (u < 0.f) { // Clamp to u == 0 and solve for v. v = B[1] / m[1][1]; v = Clamp01(v); return a + v*e1; } else if (v < 0.f) { // Clamp to v == 0 and solve for u. u = B[0] / m[0][0]; u = Clamp01(u); return a + u*e0; } else if (u+v > 1.f) { // Set v = 1-u and solve again. // u = (B[0] - m[0][0]) / (m[0][0] - m[0][1]); // mathassert(EqualAbs(u, Clamp01(u))); // u = Clamp01(u); // The solution for u must also be in the range [0,1]. // return a + u*e0; // Clamp to v = 1-u and solve again. float m_00 = m[2][2]; float m_01 = m[1][2] - m[0][2]; float m_10 = m_01; float m_11 = m[0][0] + m[1][1] - 2.f * m[0][1]; float det = m_00 * m_11 - m_01 * m_10; float b0 = m[1][1] - m[0][1] + v_p_dot_e1 - v_p_dot_e0; float b1 = -m[1][2] + v_p_dot_d; float u = m_00 * b0 + m_01 * b1; u /= det; u = Clamp01(u); float t = m_10 * b0 + m_11 * b1; t /= det; t = Clamp01(t); if (otherPt) *otherPt = lineSegment.GetPoint(t); return a + u*e0 + (1.f-u)*e1; } else { // The solution is (u, v, 0) return a + u * e0 + v * e1; } } else if (uvt.z > 1.f) { if (otherPt) *otherPt = lineSegment.b; // Clamp to t == 1 and solve again. float m_00 = m[1][1]; float m_01 = -m[0][1]; float m_10 = -m[1][0]; float m_11 = m[0][0]; float det = m_00 * m_11 - m_01 * m_10; float u = m_00 * (B[0]-m[0][2]) + m_01 * (B[1]-m[1][2]); float v = m_10 * (B[0]-m[0][2]) + m_11 * (B[1]-m[1][2]); u /= det; v /= det; if (u < 0.f) { // Clamp to u == 0 and solve again. v = (B[1] - m[1][2]) / m[1][1]; v = Clamp01(v); return a + v*e1; } else if (u > 1.f) { // Clamp to u == 1 and solve again. v = (B[1] - m[1][0] - m[1][2]) / m[1][1]; v = Clamp01(v); // The solution for v must also be in the range [0,1]. TODO: Is this guaranteed by the above? // The solution is (u,v,t)=(1,v,1). return a + e0 + v*e1; } else if (u+v > 1.f) { // Set v = 1-u and solve again. // Q(u,1-u) = a + u*e0 + e1 - u*e1 = a+e1 + u*(e0-e1) // L(1) = ls.a + t*d = ls.b // Minimize the distance |Q(u,1-u) - L(1)| = |a+e1+ls.b + u*(e0-e1)| // |K + u*(e0-e1)|^2 = (K,K) + 2*u(K,e0-e1) + u^2 * (e0-e1,e0-e1) // grad = 2*(K,e0-e1) + 2*u*(e0-e1,e0-e1) == 0 // u == (K,e1-e0) / (e0-e1,e0-e1) u = (B[0] - m[0][1] - m[0][2]) / (m[0][0] - m[0][1]); // u = Dot(a + e1 + lineSegment.b, e1 - e0) / Dot(e0-e1, e0-e1); // mathassert(EqualAbs(u, Clamp01(u))); u = Clamp01(u); return a + u*e0 + (1-u)*e1; } else { // The solution is (u, v, 1) return a + u*e0 + v*e1; } } else if (uvt.x + uvt.y > 1.f) { // Clamp to v = 1-u and solve again. float m_00 = m[2][2]; float m_01 = m[1][2] - m[0][2]; float m_10 = m_01; float m_11 = m[0][0] + m[1][1] - 2.f * m[0][1]; float det = m_00 * m_11 - m_01 * m_10; float b0 = m[1][1] - m[0][1] + v_p_dot_e1 - v_p_dot_e0; float b1 = -m[1][2] + v_p_dot_d; float u = m_00 * b0 + m_01 * b1; float t = m_10 * b0 + m_11 * b1; u /= det; t /= det; t = Clamp01(t); if (otherPt) *otherPt = lineSegment.GetPoint(t); if (u < 0.f) { // The solution is (u,v,t)=(0,1,t) return c; } if (u > 1.f) { // The solution is (u,v,t)=(1,0,t) return b; } mathassert(t >= 0.f); mathassert(t <= 1.f); return a + u*e0 + (1.f-u)*e1; } else // All parameters are within range, so the triangle and the line segment intersect, and the intersection point is the closest point. { if (otherPt) *otherPt = lineSegment.GetPoint(uvt.z); return a + uvt.x * e0 + uvt.y * e1; } }
/** For reference, see http://realtimecollisiondetection.net/blog/?p=20 . */ Sphere Sphere::OptimalEnclosingSphere(const vec &a, const vec &b, const vec &c, const vec &d) { Sphere sphere; float s,t,u; const vec ab = b-a; const vec ac = c-a; const vec ad = d-a; bool success = FitSphereThroughPoints(ab, ac, ad, s, t, u); if (!success || s < 0.f || t < 0.f || u < 0.f || s+t+u > 1.f) { sphere = OptimalEnclosingSphere(a,b,c); if (!sphere.Contains(d)) { sphere = OptimalEnclosingSphere(a,b,d); if (!sphere.Contains(c)) { sphere = OptimalEnclosingSphere(a,c,d); if (!sphere.Contains(b)) { sphere = OptimalEnclosingSphere(b,c,d); sphere.r = Max(sphere.r, a.Distance(sphere.pos) + 1e-3f); // For numerical stability, expand the radius of the sphere so it certainly contains the fourth point. assume(sphere.Contains(a)); } } } } /* // Note: Trying to approach the problem like this, like was in the triangle case, is flawed: if (s < 0.f) sphere = OptimalEnclosingSphere(a, c, d); else if (t < 0.f) sphere = OptimalEnclosingSphere(a, b, d); else if (u < 0.f) sphere = OptimalEnclosingSphere(a, b, c); else if (s + t + u > 1.f) sphere = OptimalEnclosingSphere(b, c, d); */ else // The fitted sphere is inside the convex hull of the vertices (a,b,c,d), so it must be optimal. { const vec center = s*ab + t*ac + u*ad; sphere.pos = a + center; // Mathematically, the following would be correct, but it suffers from floating point inaccuracies, // since it only tests distance against one point. //sphere.r = center.Length(); // For robustness, take the radius to be the distance to the farthest point (though the distance are all // equal). sphere.r = Sqrt(Max(sphere.pos.DistanceSq(a), sphere.pos.DistanceSq(b), sphere.pos.DistanceSq(c), sphere.pos.DistanceSq(d))); } // Allow floating point inconsistency and expand the radius by a small epsilon so that the containment tests // really contain the points (note that the points must be sufficiently near enough to the origin) sphere.r += 2.f*sEpsilon; // We test against one epsilon, so expand using 2 epsilons. #ifdef MATH_ASSERT_CORRECTNESS if (!sphere.Contains(a, sEpsilon) || !sphere.Contains(b, sEpsilon) || !sphere.Contains(c, sEpsilon) || !sphere.Contains(d, sEpsilon)) { LOGE("Pos: %s, r: %f", sphere.pos.ToString().c_str(), sphere.r); LOGE("A: %s, dist: %f", a.ToString().c_str(), a.Distance(sphere.pos)); LOGE("B: %s, dist: %f", b.ToString().c_str(), b.Distance(sphere.pos)); LOGE("C: %s, dist: %f", c.ToString().c_str(), c.Distance(sphere.pos)); LOGE("D: %s, dist: %f", d.ToString().c_str(), d.Distance(sphere.pos)); mathassert(false); } #endif return sphere; }
/** For reference, see http://realtimecollisiondetection.net/blog/?p=20 . */ Sphere Sphere::OptimalEnclosingSphere(const vec &a, const vec &b, const vec &c) { Sphere sphere; vec ab = b-a; vec ac = c-a; float s, t; bool areCollinear = ab.Cross(ac).LengthSq() < 1e-4f; // Manually test that we don't try to fit sphere to three collinear points. bool success = !areCollinear && FitSphereThroughPoints(ab, ac, s, t); if (!success || Abs(s) > 10000.f || Abs(t) > 10000.f) // If s and t are very far from the triangle, do a manual box fitting for numerical stability. { vec minPt = Min(a, b, c); vec maxPt = Max(a, b, c); sphere.pos = (minPt + maxPt) * 0.5f; sphere.r = sphere.pos.Distance(minPt); } else if (s < 0.f) { sphere.pos = (a + c) * 0.5f; sphere.r = a.Distance(c) * 0.5f; sphere.r = Max(sphere.r, b.Distance(sphere.pos)); // For numerical stability, expand the radius of the sphere so it certainly contains the third point. } else if (t < 0.f) { sphere.pos = (a + b) * 0.5f; sphere.r = a.Distance(b) * 0.5f; sphere.r = Max(sphere.r, c.Distance(sphere.pos)); // For numerical stability, expand the radius of the sphere so it certainly contains the third point. } else if (s+t > 1.f) { sphere.pos = (b + c) * 0.5f; sphere.r = b.Distance(c) * 0.5f; sphere.r = Max(sphere.r, a.Distance(sphere.pos)); // For numerical stability, expand the radius of the sphere so it certainly contains the third point. } else { const vec center = s*ab + t*ac; sphere.pos = a + center; // Mathematically, the following would be correct, but it suffers from floating point inaccuracies, // since it only tests distance against one point. //sphere.r = center.Length(); // For robustness, take the radius to be the distance to the farthest point (though the distance are all // equal). sphere.r = Sqrt(Max(sphere.pos.DistanceSq(a), sphere.pos.DistanceSq(b), sphere.pos.DistanceSq(c))); } // Allow floating point inconsistency and expand the radius by a small epsilon so that the containment tests // really contain the points (note that the points must be sufficiently near enough to the origin) sphere.r += 2.f * sEpsilon; // We test against one epsilon, so expand by two epsilons. #ifdef MATH_ASSERT_CORRECTNESS if (!sphere.Contains(a, sEpsilon) || !sphere.Contains(b, sEpsilon) || !sphere.Contains(c, sEpsilon)) { LOGE("Pos: %s, r: %f", sphere.pos.ToString().c_str(), sphere.r); LOGE("A: %s, dist: %f", a.ToString().c_str(), a.Distance(sphere.pos)); LOGE("B: %s, dist: %f", b.ToString().c_str(), b.Distance(sphere.pos)); LOGE("C: %s, dist: %f", c.ToString().c_str(), c.Distance(sphere.pos)); mathassert(false); } #endif return sphere; }