Celestia/src/celmath/intersect.h at master · httpsgithu/Celestia · 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
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
118
119
120
121
122
123
124
125
126
127
128
129
// intersect.h
//
// Copyright (C) 2002, Chris Laurel <claurel@shatters.net>
//
// Intersection calculation for various geometric objects.
//
// This program is free software; you can redistribute it and/or
// modify it under the terms of the GNU General Public License
// as published by the Free Software Foundation; either version 2
// of the License, or (at your option) any later version.

#ifndef _CELMATH_INTERSECT_H_
#define _CELMATH_INTERSECT_H_

#include "ray.h"
#include "sphere.h"
#include "ellipsoid.h"
#include "mathlib.h"
#include <Eigen/Core>

namespace celmath
{

template<class T> bool testIntersection(const Ray3<T>& ray,
                                        const Sphere<T>& sphere,
                                        T& distance)
{
    Eigen::Matrix<T, 3, 1> diff = ray.origin - sphere.center;
    T s = (T) 1.0 / square(sphere.radius);
    T a = ray.direction.squaredNorm() * s;
    T b = ray.direction.dot(diff) * s;
    T c = diff.squaredNorm() * s - (T) 1.0;
    T disc = b * b - a * c;
    if (disc < 0.0)
        return false;

    disc = (T) sqrt(disc);
    T sol0 = (-b + disc) / a;
    T sol1 = (-b - disc) / a;

    if (sol0 > 0)
    {
        if (sol0 < sol1 || sol1 < 0)
            distance = sol0;
        else
            distance = sol1;
        return true;
    }
    else if (sol1 > 0)
    {
        distance = sol1;
        return true;
    }
    else
    {
        return false;
    }
}


template<class T> bool testIntersection(const Ray3<T>& ray,
                                        const Sphere<T>& sphere,
                                        T& distanceToTester,
                                        T& cosAngleToCenter)
{
    if (testIntersection(ray, sphere, distanceToTester))
    {
        cosAngleToCenter = (sphere.center - ray.origin).dot(ray.direction) / (sphere.center - ray.origin).norm();
        return true;
    }
    return false;
}


template<class T> bool testIntersection(const Ray3<T>& ray,
                                        const Ellipsoid<T>& e,
                                        T& distance)
{
    Eigen::Matrix<T, 3, 1> diff = ray.origin - e.center;
    Eigen::Matrix<T, 3, 1> s = e.axes.cwiseInverse().array().square();
    Eigen::Matrix<T, 3, 1> sdir = ray.direction.cwiseProduct(s);
    Eigen::Matrix<T, 3, 1> sdiff = diff.cwiseProduct(s);

    T a = ray.direction.dot(sdir);
    T b = ray.direction.dot(sdiff);
    T c = diff.dot(sdiff) - (T) 1.0;
    T disc = b * b - a * c;
    if (disc < 0.0)
        return false;

    disc = (T) sqrt(disc);
    T sol0 = (-b + disc) / a;
    T sol1 = (-b - disc) / a;

    if (sol0 > 0)
    {
        if (sol0 < sol1 || sol1 < 0)
            distance = sol0;
        else
            distance = sol1;
        return true;
    }
    else if (sol1 > 0)
    {
        distance = sol1;
        return true;
    }
    else
    {
        return false;
    }
}


template<class T> bool testIntersection(const Ray3<T>& ray,
                                        const Ellipsoid<T>& ellipsoid,
                                        T& distanceToTester,
                                        T& cosAngleToCenter)
{
    if (testIntersection(ray, ellipsoid, distanceToTester))
    {
        cosAngleToCenter = (ellipsoid.center - ray.origin).dot(ray.direction) / (ellipsoid.center - ray.origin).norm();
        return true;
    }
    return false;
}
} // namespace celmath

#endif // _CELMATH_INTERSECT_H_