1
  2
  3
  4
  5
  6
  7
  8
  9
 10
 11
 12
 13
 14
 15
 16
 17
 18
 19
 20
 21
 22
 23
 24
 25
 26
 27
 28
 29
 30
 31
 32
 33
 34
 35
 36
 37
 38
 39
 40
 41
 42
 43
 44
 45
 46
 47
 48
 49
 50
 51
 52
 53
 54
 55
 56
 57
 58
 59
 60
 61
 62
 63
 64
 65
 66
 67
 68
 69
 70
 71
 72
 73
 74
 75
 76
 77
 78
 79
 80
 81
 82
 83
 84
 85
 86
 87
 88
 89
 90
 91
 92
 93
 94
 95
 96
 97
 98
 99
100
101
102
103
104
105
106
107
108
109
110
111
112
113
114
115
116
117

// Copyright 2015 Nicholas Bishop
//
// Closest-points-between-lines method adapted from "Real-Time
// Collision Detection" by Christer Ericson, published by Morgan
// Kaufmann Publishers, Copyright 2005 Elsevier Inc
//
// Licensed under the Apache License, Version 2.0 (the "License");
// you may not use this file except in compliance with the License.
// You may obtain a copy of the License at
//
//     http://www.apache.org/licenses/LICENSE-2.0
//
// Unless required by applicable law or agreed to in writing, software
// distributed under the License is distributed on an "AS IS" BASIS,
// WITHOUT WARRANTIES OR CONDITIONS OF ANY KIND, either express or implied.
// See the License for the specific language governing permissions and
// limitations under the License.

use vec3f::Vec3f;

/// Line of infinite length represented by two distinct points it
/// passes through.
#[derive(Clone, Copy, Debug, PartialEq)]
pub struct Line3f {
    pub points: (Vec3f, Vec3f)
}

impl Line3f {
    /// Create a new line. Return None if the input points are
    /// identical.
    pub fn new(a: Vec3f, b: Vec3f) -> Option<Line3f> {
        if a == b {
            None
        }
        else {
            Some(Line3f {
                points: (a, b)
            })
        }
    }

    /// Find the closest points between two lines. The result is a
    /// pair of parametric points, the first for `self` and the second
    /// for `line`. If the lines are parallel then None is
    /// returned.
    ///
    /// Adapted from "Real-Time Collision Detection" by Christer
    /// Ericson, published by Morgan Kaufmann Publishers, Copyright
    /// 2005 Elsevier Inc
    pub fn closest_points_between_lines(self,
                                        line: Line3f) -> Option<(f32, f32)> {
        let d1 = self.points.1 - self.points.0;
        let d2 = line.points.1 - line.points.0;
        let r = self.points.0 - line.points.0;

        let a = d1.dot(d1);
        let b = d1.dot(d2);
        let c = d1.dot(r);
        let e = d2.dot(d2);
        let f = d2.dot(r);

        let d = a * e - b * b;
        if d == 0.0 {
            None
        }
        else {
            let s = (b * f - c * e) / d;
            let t = (a * f - b * c) / d;
            Some((s, t))
        }
    }
}

#[cfg(test)]
mod test {
    use super::*;
    use vec3f::vec3f;

    #[test]
    fn test_line3f() {
        let a = vec3f(0, 0, 0);
        let b = vec3f(1, 0, 0);
        assert_eq!(Line3f::new(a, a), None);
        assert_eq!(Line3f::new(a, b).unwrap(), Line3f { points: (a, b) } );
    }

    #[test]
    fn test_closest_points_between_lines() {
        let a = vec3f(0, 0, 0);
        let b = vec3f(4, 0, 0);

        let c = vec3f(2, 0, 0);
        let d = vec3f(2, 2, 0);

        let l1 = Line3f::new(a, b).unwrap();
        let l2 = Line3f::new(c, d).unwrap();

        // Intersection
        assert_eq!(l1.closest_points_between_lines(l2).unwrap(), (0.5, 0.0));

        // Coincident
        assert_eq!(l1.closest_points_between_lines(l1), None);

        // Parallel, not coincident
        let e = vec3f(0, 0, 1);
        let f = vec3f(4, 0, 1);
        let l3 = Line3f::new(e, f).unwrap();
        assert_eq!(l1.closest_points_between_lines(l3), None);

        // No intersection, not parallel
        let g = vec3f(0, 4, 1);
        let h = vec3f(0, 0, 1);
        let l4 = Line3f::new(g, h).unwrap();
        assert_eq!(l1.closest_points_between_lines(l4).unwrap(),
                   (0.0, 1.0));
    }
}