float OBB::Distance(const float3 &point) const { ///\todo This code can be optimized a bit. See Christer Ericson's Real-Time Collision Detection, /// p.134. float3 closestPoint = ClosestPoint(point); return point.Distance(closestPoint); }
float3 SceneInteract::RaycastFurthestIntersect(const float3 &from, const Vector<float3> &to, unsigned layerMask) const { float3 intersection = float3::nan; Scene* scene = framework->Module<UrhoRenderer>()->MainCameraScene(); GraphicsWorld* world = scene ? scene->Subsystem<GraphicsWorld>().Get() : nullptr; if (!world) return intersection; Ray ray; ray.pos = from; float furthest = 0.0f; for (int i=0, len=to.Size(); i<len; ++i) { ray.dir = to[i].Sub(from).Normalized(); // We use raycast all as there might be multiple entities in between 'from' and 'to' // and this function should return the closest hit to the 'to' target. float maxDistance = from.Distance(to[i]); RayQueryResultVector results = world->RaycastAll(ray, layerMask, maxDistance); if (!results.Empty()) { // Last result is picked as the results are already ordered by distance. RayQueryResult& result = results.Back(); if (result.t > furthest) { furthest = result.t; intersection = result.pos; } } } return intersection; }
float Sphere::MaxDistance(const float3 &point) const { return point.Distance(pos) + r; }
/** For reference, see http://realtimecollisiondetection.net/blog/?p=20 . */ Sphere Sphere::OptimalEnclosingSphere(const float3 &a, const float3 &b, const float3 &c) { Sphere sphere; float3 ab = b-a; float3 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. { float3 minPt = Min(a, b, c); float3 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 float3 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 * epsilon; // We test against one epsilon, so expand by two epsilons. #ifdef MATH_ASSERT_CORRECTNESS if (!sphere.Contains(a, epsilon) || !sphere.Contains(b, epsilon) || !sphere.Contains(c, epsilon)) { 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; }
/** For reference, see http://realtimecollisiondetection.net/blog/?p=20 . */ Sphere Sphere::OptimalEnclosingSphere(const float3 &a, const float3 &b, const float3 &c, const float3 &d) { Sphere sphere; float s,t,u; const float3 ab = b-a; const float3 ac = c-a; const float3 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 float3 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*epsilon; // We test against one epsilon, so expand using 2 epsilons. #ifdef MATH_ASSERT_CORRECTNESS if (!sphere.Contains(a, epsilon) || !sphere.Contains(b, epsilon) || !sphere.Contains(c, epsilon) || !sphere.Contains(d, epsilon)) { 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; }