diff options
Diffstat (limited to 'dep/g3dlite/source/LineSegment.cpp')
| -rw-r--r-- | dep/g3dlite/source/LineSegment.cpp | 236 |
1 files changed, 236 insertions, 0 deletions
diff --git a/dep/g3dlite/source/LineSegment.cpp b/dep/g3dlite/source/LineSegment.cpp new file mode 100644 index 00000000000..754600ad554 --- /dev/null +++ b/dep/g3dlite/source/LineSegment.cpp @@ -0,0 +1,236 @@ +/** + @file LineSegment.cpp + + @maintainer Morgan McGuire, http://graphics.cs.williams.edu + + @created 2003-02-08 + @edited 2008-02-02 + */ + +#include "G3D/platform.h" +#include "G3D/LineSegment.h" +#include "G3D/Sphere.h" +#include "G3D/debug.h" + +namespace G3D { + + +Vector3 LineSegment::closestPoint(const Vector3& p) const { + + // The vector from the end of the capsule to the point in question. + Vector3 v(p - _point); + + // Projection of v onto the line segment scaled by + // the length of direction. + float t = direction.dot(v); + + // Avoid some square roots. Derivation: + // t/direction.length() <= direction.length() + // t <= direction.squaredLength() + + if ((t >= 0) && (t <= direction.squaredMagnitude())) { + + // The point falls within the segment. Normalize direction, + // divide t by the length of direction. + return _point + direction * t / direction.squaredMagnitude(); + + } else { + + // The point does not fall within the segment; see which end is closer. + + // Distance from 0, squared + float d0Squared = v.squaredMagnitude(); + + // Distance from 1, squared + float d1Squared = (v - direction).squaredMagnitude(); + + if (d0Squared < d1Squared) { + + // Point 0 is closer + return _point; + + } else { + + // Point 1 is closer + return _point + direction; + + } + } + +} + +Vector3 LineSegment::point(int i) const { + switch (i) { + case 0: + return _point; + + case 1: + return _point + direction; + + default: + debugAssertM(i == 0 || i == 1, "Argument to point must be 0 or 1"); + return _point; + } +} + + +bool LineSegment::intersectsSolidSphere(const class Sphere& s) const { + return distanceSquared(s.center) <= square(s.radius); +} + + +LineSegment::LineSegment(class BinaryInput& b) { + deserialize(b); +} + + +void LineSegment::serialize(class BinaryOutput& b) const { + _point.serialize(b); + direction.serialize(b); +} + + +void LineSegment::deserialize(class BinaryInput& b) { + _point.deserialize(b); + direction.deserialize(b); +} + + +Vector3 LineSegment::randomPoint() const { + return _point + uniformRandom(0, 1) * direction; +} + + +///////////////////////////////////////////////////////////////////////////////////// + +LineSegment2D LineSegment2D::fromTwoPoints(const Vector2& p0, const Vector2& p1) { + LineSegment2D s; + s.m_origin = p0; + s.m_direction = p1 - p0; + s.m_length = s.m_direction.length(); + return s; +} + + +Vector2 LineSegment2D::point(int i) const { + debugAssert(i == 0 || i == 1); + if (i == 0) { + return m_origin; + } else { + return m_direction + m_origin; + } +} + + +Vector2 LineSegment2D::closestPoint(const Vector2& Q) const { + // Two constants that appear in the result + const Vector2 k1(m_origin - Q); + const Vector2& k2 = m_direction; + + if (fuzzyEq(m_length, 0)) { + // This line segment has no length + return m_origin; + } + + // Time [0, 1] at which we hit the closest point travelling from p0 to p1. + // Derivation can be obtained by minimizing the expression + // ||P0 + (P1 - P0)t - Q||. + const float t = -k1.dot(k2) / (m_length * m_length); + + if (t < 0) { + // Clipped to low end point + return m_origin; + } else if (t > 1) { + // Clipped to high end point + return m_origin + m_direction; + } else { + // Subsitute into the line equation to find + // the point on the segment. + return m_origin + k2 * t; + } +} + + +float LineSegment2D::distance(const Vector2& p) const { + Vector2 closest = closestPoint(p); + return (closest - p).length(); +} + + +float LineSegment2D::length() const { + return m_length; +} + + +Vector2 LineSegment2D::intersection(const LineSegment2D& other) const { + + if ((m_origin == other.m_origin) || + (m_origin == other.m_origin + other.m_direction)) { + return m_origin; + } + + if (m_origin + m_direction == other.m_origin) { + return other.m_origin; + } + + // Note: Now that we've checked the endpoints, all other parallel lines can now be assumed + // to not intersect (within numerical precision) + + Vector2 dir1 = m_direction; + Vector2 dir2 = other.m_direction; + Vector2 origin1 = m_origin; + Vector2 origin2 = other.m_origin; + + if (dir1.x == 0) { + // Avoid an upcoming divide by zero + dir1 = dir1.yx(); + dir2 = dir2.yx(); + origin1 = origin1.yx(); + origin2 = origin2.yx(); + } + + // t1 = ((other.m_origin.x - m_origin.x) + other.m_direction.x * t2) / m_direction.x + // + // ((other.m_origin.x - m_origin.x) + other.m_direction.x * t2) * m_direction.y / m_direction.x = + // (other.m_origin.y - m_origin.y) + other.m_direction.y * t2 + // + // m = m_direction.y / m_direction.x + // d = other.m_origin - m_origin + // + // (d.x + other.m_direction.x * t2) * m = d.y + other.m_direction.y * t2 + // + // d.x * m + other.m_direction.x * m * t2 = d.y + other.m_direction.y * t2 + // + // d.x * m - d.y = (other.m_direction.y - other.m_direction.x * m) * t2 + // + // (d.x * m - d.y) / (other.m_direction.y - other.m_direction.x * m) = t2 + // + + Vector2 d = origin2 - origin1; + float m = dir1.y / dir1.x; + + float t2 = (d.x * m - d.y) / (dir2.y - dir2.x * m); + if (! isFinite(t2)) { + // Parallel lines: no intersection + return Vector2::inf(); + } + + if ((t2 < 0.0f) || (t2 > 1.0f)) { + // Intersection occurs past the end of the line segments + return Vector2::inf(); + } + + float t1 = (d.x + dir2.x * t2) / dir1.x; + if ((t1 < 0.0f) || (t1 > 1.0f)) { + // Intersection occurs past the end of the line segments + return Vector2::inf(); + } + + // Return the intersection point (computed from non-transposed + // variables even if we flipped above) + return m_origin + m_direction * t1; + +} + +} + |