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;
}
