Triangulation/R3Graph.cpp at master · diovch/Triangulation · GitHub

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
#include "R3Graph.h"

namespace R3Graph {

bool intersectPlanes(
    const R3Point& p0, const R3Vector& n0,
    const R3Point& p1, const R3Vector& n1,
    R3Point& p, R3Vector& v
) {
    if (n0.area(n1) <= R3_EPSILON)
        return false;
    v = n0.vectorProduct(n1).normalize();
    R3Vector v1 = n0.vectorProduct(v);

    // p = p0 + v1*t
    // ((p0 + v1*t) - p1, n1) = 0
    // (p0 - p1, n1) + (v1, n1)*t = 0
    // t = (p1 - p0, n1) / (v1, n1)
    double s = v1.scalarProduct(n1);
    if (fabs(s) <= R3_EPSILON)
        return false;
    double t = (p1 - p0).scalarProduct(n1) / s;
    p = p0 + v1 * t;
    return true;
}

bool intersectPlaneAndLine(
    const R3Point& p0, const R3Vector& n,
    const R3Point& p1, const R3Vector& v,
    R3Point& p
) {
    double s = n.scalarProduct(v);
    if (fabs(s) <= R3_EPSILON)
        return false;
    // p = p1 + v*t;
    // (p - p0, n) = 0
    // (p1 + v*t - p0, n) = 0
    // (p0 - p1, n) = (v, n)*t
    // t = (p0 - p1, n) / (v, n)
    double t = (p0 - p1).scalarProduct(n) / s;
    p = p1 + v*t;
    return true;
}

double R3Vector::signedSolidAngle(
    const R3Vector& a,
    const R3Vector& b,
    const R3Vector& c
) {
    // Oosterom and Strackee formula
    double v = R3Vector::signedVolume(a, b, c);
    double lena = a.length();
    double lenb = b.length();
    double lenc = c.length();
    double d = lena * lenb * lenc +
        (a * b) * lenc + (a * c) * lenb + (b * c) * lena;
    return 2. * atan(v / d);
}

} // end of namespace R3Graph