+
+ Line() {}
+
+ Line(const Vector& point1, const Vector& point2) :
+ a(point1),
+ b(point2) {}
+
+
+ Vector getDirection() const
+ {
+ return b - a;
+ }
+
+ Scalar getLength() const
+ {
+ return getDirection().length();
+ }
+
+
+ bool intersect(const Line& other, Contact<D>& hit) const
+ {
+ Scalar d = (other.b[1] - other.a[1]) * (b[0] - a[0]) -
+ (other.b[0] - other.a[0]) * (b[1] - a[1]);
+
+ if (d == SCALAR(0.0)) return false; // lines are parallel
+ // ignoring the (somewhat remote) possibility of coincidence
+
+ Scalar m = ((other.b[0] - other.a[0]) * (a[1] - other.a[1]) -
+ (other.b[1] - other.a[1]) * (a[0] - other.a[0])) / d;
+
+ Scalar n = ((b[0] - a[0]) * (b[1] - other.a[1]) -
+ (b[1] - a[1]) * (b[0] - other.a[0])) / d;
+
+ if (m < SCALAR(0.0) || m > SCALAR(1.0) || // not intersecting
+ n < SCALAR(0.0) || n > SCALAR(1.0)) return false;
+
+ Vector2 tangent = b - a;
+ Vector2 normal = cml::perp(tangent).normalize();
+
+ if (cml::dot(normal, other.a - other.b) < SCALAR(0.0))
+ {
+ normal = -normal;
+ }
+
+ hit.point = a + m * tangent;
+ hit.normal = normal;
+ hit.distance = (other.b - hit.point).length();
+
+ return true;
+ }
+
+ bool intersect(const Sphere<D>& other, Contact<D>& hit) const
+ {
+ Vector surface = b - a;
+ Vector toPoint = other.point - a;
+
+ Scalar surfaceLength = surface.length();
+ surface.normalize();
+
+ Scalar projection = cml::dot(surface, toPoint);
+
+ if (projection < SCALAR(0.0) || projection > surfaceLength)
+ {
+ // try endpoints
+
+ if (other.intersect(a, hit))
+ {
+ hit.normal = -hit.normal;
+ hit.point = a;
+ return true;
+ }
+ else if (other.intersect(b, hit))
+ {
+ hit.normal = -hit.normal;
+ hit.point = b;
+ return true;
+ }
+
+ return false;
+ }
+
+ Vector point = a + surface * projection;
+ Vector normal = other.point - point;
+
+ Scalar distance = normal.length();
+
+ if (distance > other.radius) false; // not intersecting
+
+ normal.normalize();
+
+ hit.distance = other.radius - distance;
+ hit.point = point;
+ hit.normal = normal;
+
+ return true;
+ }
+
+
+ bool intersectRay(const Ray<2>& ray, Ray<2>::Contact& hit) const