src/utilities/geometry/Plane.cpp
| Line | Branch | Exec | Source |
|---|---|---|---|
| 1 | /*********************************************************************************************************************** | ||
| 2 | * OpenStudio(R), Copyright (c) Alliance for Energy Innovation, LLC. | ||
| 3 | * See also https://openstudio.net/license | ||
| 4 | ***********************************************************************************************************************/ | ||
| 5 | |||
| 6 | #include "Plane.hpp" | ||
| 7 | |||
| 8 | #include "Point3d.hpp" | ||
| 9 | #include "Vector3d.hpp" | ||
| 10 | #include "Geometry.hpp" | ||
| 11 | #include "../data/Vector.hpp" | ||
| 12 | #include "../data/Matrix.hpp" | ||
| 13 | |||
| 14 | #include "../core/Assert.hpp" | ||
| 15 | |||
| 16 | #include <cmath> | ||
| 17 | #include <iomanip> | ||
| 18 | |||
| 19 | namespace openstudio { | ||
| 20 | |||
| 21 | 1043137 | Plane::Plane(const Point3d& point, const Vector3d& outwardNormal) { | |
| 22 |
1/1✓ Branch 1 taken 1043137 times.
|
1043137 | Vector3d thisNormal(outwardNormal); |
| 23 |
3/3✓ Branch 1 taken 1043137 times.
✓ Branch 3 taken 1 time.
✓ Branch 4 taken 1043136 times.
|
1043137 | if (!thisNormal.normalize()) { |
| 24 |
13/13✓ Branch 1 taken 1 time.
✓ Branch 4 taken 1 time.
✓ Branch 7 taken 1 time.
✓ Branch 10 taken 1 time.
✓ Branch 13 taken 1 time.
✓ Branch 19 taken 1 time.
✓ Branch 22 taken 1 time.
✓ Branch 25 taken 1 time.
✓ Branch 28 taken 1 time.
✓ Branch 31 taken 1 time.
✓ Branch 34 taken 1 time.
✓ Branch 38 taken 1 time.
✓ Branch 41 taken 1 time.
|
1 | LOG_AND_THROW("Cannot initialize plane because normal is undefined"); |
| 25 | } | ||
| 26 | |||
| 27 |
1/1✓ Branch 1 taken 1043136 times.
|
1043136 | m_a = thisNormal.x(); |
| 28 |
1/1✓ Branch 1 taken 1043136 times.
|
1043136 | m_b = thisNormal.y(); |
| 29 |
1/1✓ Branch 1 taken 1043136 times.
|
1043136 | m_c = thisNormal.z(); |
| 30 |
6/6✓ Branch 1 taken 1043136 times.
✓ Branch 4 taken 1043136 times.
✓ Branch 7 taken 1043136 times.
✓ Branch 10 taken 1043136 times.
✓ Branch 13 taken 1043136 times.
✓ Branch 16 taken 1043136 times.
|
1043136 | m_d = -thisNormal.x() * point.x() - thisNormal.y() * point.y() - thisNormal.z() * point.z(); |
| 31 | 1043137 | } | |
| 32 | |||
| 33 | 266052 | double det3x3(const Matrix& m) { | |
| 34 | 266052 | const double det = m(0, 0) * m(1, 1) * m(2, 2) + m(0, 1) * m(1, 2) * m(2, 0) + m(0, 2) * m(1, 0) * m(2, 1) - m(0, 2) * m(1, 1) * m(2, 0) | |
| 35 | 266052 | - m(0, 1) * m(1, 0) * m(2, 2) - m(0, 0) * m(1, 2) * m(2, 1); | |
| 36 | 266052 | return det; | |
| 37 | } | ||
| 38 | |||
| 39 | 90544 | Plane::Plane(const std::vector<Point3d>& points) : m_a(0.0), m_b(0.0), m_c(0.0), m_d(0.0) { | |
| 40 | 90544 | const size_t N = points.size(); | |
| 41 |
2/2✓ Branch 0 taken 3 times.
✓ Branch 1 taken 90541 times.
|
90544 | if (N < 3) { |
| 42 |
13/13✓ Branch 1 taken 3 times.
✓ Branch 4 taken 3 times.
✓ Branch 7 taken 3 times.
✓ Branch 10 taken 3 times.
✓ Branch 13 taken 3 times.
✓ Branch 19 taken 3 times.
✓ Branch 22 taken 3 times.
✓ Branch 25 taken 3 times.
✓ Branch 28 taken 3 times.
✓ Branch 31 taken 3 times.
✓ Branch 34 taken 3 times.
✓ Branch 38 taken 3 times.
✓ Branch 41 taken 3 times.
|
3 | LOG_AND_THROW("Cannot compute plane with fewer than three points"); |
| 43 |
2/2✓ Branch 0 taken 1857 times.
✓ Branch 1 taken 88684 times.
|
90541 | } else if (N == 3) { |
| 44 | |||
| 45 | // duplicates code in point and normal ctor, points[1] is the point and (a x b) is the normal | ||
| 46 | 1857 | const Point3d& point = points[1]; | |
| 47 |
1/1✓ Branch 3 taken 1857 times.
|
1857 | const Vector3d a = points[1] - points[0]; |
| 48 |
1/1✓ Branch 3 taken 1857 times.
|
1857 | const Vector3d b = points[2] - points[1]; |
| 49 |
1/1✓ Branch 1 taken 1857 times.
|
1857 | Vector3d thisNormal = a.cross(b); |
| 50 | |||
| 51 |
3/3✓ Branch 1 taken 1857 times.
✓ Branch 3 taken 2 times.
✓ Branch 4 taken 1855 times.
|
1857 | if (!thisNormal.normalize()) { |
| 52 |
13/13✓ Branch 1 taken 2 times.
✓ Branch 4 taken 2 times.
✓ Branch 7 taken 2 times.
✓ Branch 10 taken 2 times.
✓ Branch 13 taken 2 times.
✓ Branch 19 taken 2 times.
✓ Branch 22 taken 2 times.
✓ Branch 25 taken 2 times.
✓ Branch 28 taken 2 times.
✓ Branch 31 taken 2 times.
✓ Branch 34 taken 2 times.
✓ Branch 38 taken 2 times.
✓ Branch 41 taken 2 times.
|
2 | LOG_AND_THROW("Cannot initialize plane because normal is undefined"); |
| 53 | } | ||
| 54 | |||
| 55 |
1/1✓ Branch 1 taken 1855 times.
|
1855 | m_a = thisNormal.x(); |
| 56 |
1/1✓ Branch 1 taken 1855 times.
|
1855 | m_b = thisNormal.y(); |
| 57 |
1/1✓ Branch 1 taken 1855 times.
|
1855 | m_c = thisNormal.z(); |
| 58 |
6/6✓ Branch 1 taken 1855 times.
✓ Branch 4 taken 1855 times.
✓ Branch 7 taken 1855 times.
✓ Branch 10 taken 1855 times.
✓ Branch 13 taken 1855 times.
✓ Branch 16 taken 1855 times.
|
1855 | m_d = -thisNormal.x() * point.x() - thisNormal.y() * point.y() - thisNormal.z() * point.z(); |
| 59 | |||
| 60 | 1861 | } else { | |
| 61 | |||
| 62 | 88684 | bool foundSolution = false; | |
| 63 | // 0.0001 was too big for the determinant tolerance, 1e-12 was too small | ||
| 64 | 88684 | double maxDet = 1e-8; | |
| 65 | |||
| 66 | // solve the equation ax+by+cz+d=0 in a few different ways, keep the best one | ||
| 67 | |||
| 68 | { | ||
| 69 | // Ax = b, x = [a/c; b/c; d/c] | ||
| 70 |
1/1✓ Branch 1 taken 88684 times.
|
88684 | Matrix A(N, 3); |
| 71 |
1/1✓ Branch 1 taken 88684 times.
|
88684 | Matrix At(3, N); |
| 72 |
1/1✓ Branch 1 taken 88684 times.
|
88684 | Vector b(N); |
| 73 |
2/2✓ Branch 0 taken 400580 times.
✓ Branch 1 taken 88684 times.
|
489264 | for (unsigned i = 0; i < N; ++i) { |
| 74 |
3/3✓ Branch 2 taken 400580 times.
✓ Branch 6 taken 400580 times.
✓ Branch 9 taken 400580 times.
|
400580 | A(i, 0) = points[i].x() - points[0].x(); |
| 75 |
3/3✓ Branch 2 taken 400580 times.
✓ Branch 6 taken 400580 times.
✓ Branch 9 taken 400580 times.
|
400580 | A(i, 1) = points[i].y() - points[0].y(); |
| 76 |
1/1✓ Branch 1 taken 400580 times.
|
400580 | A(i, 2) = 1.0; |
| 77 |
2/2✓ Branch 1 taken 400580 times.
✓ Branch 4 taken 400580 times.
|
400580 | At(0, i) = A(i, 0); |
| 78 |
2/2✓ Branch 1 taken 400580 times.
✓ Branch 4 taken 400580 times.
|
400580 | At(1, i) = A(i, 1); |
| 79 |
2/2✓ Branch 1 taken 400580 times.
✓ Branch 4 taken 400580 times.
|
400580 | At(2, i) = A(i, 2); |
| 80 |
3/3✓ Branch 2 taken 400580 times.
✓ Branch 6 taken 400580 times.
✓ Branch 9 taken 400580 times.
|
400580 | b[i] = -(points[i].z() - points[0].z()); |
| 81 | } | ||
| 82 | |||
| 83 |
1/1✓ Branch 1 taken 88684 times.
|
88684 | const Matrix AtA = prod(At, A); |
| 84 |
1/1✓ Branch 1 taken 88684 times.
|
88684 | const double det = det3x3(AtA); // always positive for A'*A |
| 85 |
2/2✓ Branch 0 taken 23125 times.
✓ Branch 1 taken 65559 times.
|
88684 | if (det > maxDet) { |
| 86 |
1/1✓ Branch 1 taken 23125 times.
|
23125 | Matrix AtAInv(3, 3); |
| 87 |
1/1✓ Branch 1 taken 23125 times.
|
23125 | const bool test = invert(AtA, AtAInv); |
| 88 |
2/2✓ Branch 0 taken 23107 times.
✓ Branch 1 taken 18 times.
|
23125 | if (test) { |
| 89 | 23107 | maxDet = det; | |
| 90 |
2/2✓ Branch 1 taken 23107 times.
✓ Branch 4 taken 23107 times.
|
23107 | Vector x = prod(prod(AtAInv, At), b); |
| 91 |
1/1✓ Branch 1 taken 23107 times.
|
23107 | const double a_c = x[0]; |
| 92 |
1/1✓ Branch 1 taken 23107 times.
|
23107 | const double b_c = x[1]; |
| 93 |
1/1✓ Branch 1 taken 23107 times.
|
23107 | const double d_c = x[2]; |
| 94 | |||
| 95 | // a = a_c*c | ||
| 96 | // b = b_c*c | ||
| 97 | // d = d_c*c | ||
| 98 | // a^2 + b^2 + c^2 = 1 | ||
| 99 | // c^2*(a_c^2 + b_c^2 + 1) = 1 | ||
| 100 | |||
| 101 | 23107 | m_c = 1.0 / std::sqrt(a_c * a_c + b_c * b_c + 1.0); | |
| 102 | 23107 | m_a = a_c * m_c; | |
| 103 | 23107 | m_b = b_c * m_c; | |
| 104 |
3/3✓ Branch 2 taken 23107 times.
✓ Branch 6 taken 23107 times.
✓ Branch 10 taken 23107 times.
|
23107 | m_d = d_c * m_c - m_a * points[0].x() - m_b * points[0].y() - m_c * points[0].z(); |
| 105 | 23107 | foundSolution = true; | |
| 106 | 23107 | } | |
| 107 | 23125 | } | |
| 108 | 88684 | } | |
| 109 | |||
| 110 | { | ||
| 111 | // Ax = b, x = [a/b; c/b; d/b] | ||
| 112 |
1/1✓ Branch 1 taken 88684 times.
|
88684 | Matrix A(N, 3); |
| 113 |
1/1✓ Branch 1 taken 88684 times.
|
88684 | Matrix At(3, N); |
| 114 |
1/1✓ Branch 1 taken 88684 times.
|
88684 | Vector b(N); |
| 115 |
2/2✓ Branch 0 taken 400580 times.
✓ Branch 1 taken 88684 times.
|
489264 | for (unsigned i = 0; i < N; ++i) { |
| 116 |
3/3✓ Branch 2 taken 400580 times.
✓ Branch 6 taken 400580 times.
✓ Branch 9 taken 400580 times.
|
400580 | A(i, 0) = points[i].x() - points[0].x(); |
| 117 |
3/3✓ Branch 2 taken 400580 times.
✓ Branch 6 taken 400580 times.
✓ Branch 9 taken 400580 times.
|
400580 | A(i, 1) = points[i].z() - points[0].z(); |
| 118 |
1/1✓ Branch 1 taken 400580 times.
|
400580 | A(i, 2) = 1.0; |
| 119 |
2/2✓ Branch 1 taken 400580 times.
✓ Branch 4 taken 400580 times.
|
400580 | At(0, i) = A(i, 0); |
| 120 |
2/2✓ Branch 1 taken 400580 times.
✓ Branch 4 taken 400580 times.
|
400580 | At(1, i) = A(i, 1); |
| 121 |
2/2✓ Branch 1 taken 400580 times.
✓ Branch 4 taken 400580 times.
|
400580 | At(2, i) = A(i, 2); |
| 122 |
3/3✓ Branch 2 taken 400580 times.
✓ Branch 6 taken 400580 times.
✓ Branch 9 taken 400580 times.
|
400580 | b[i] = -(points[i].y() - points[0].y()); |
| 123 | } | ||
| 124 | |||
| 125 |
1/1✓ Branch 1 taken 88684 times.
|
88684 | const Matrix AtA = prod(At, A); |
| 126 |
1/1✓ Branch 1 taken 88684 times.
|
88684 | const double det = det3x3(AtA); // always positive for A'*A |
| 127 |
2/2✓ Branch 0 taken 53452 times.
✓ Branch 1 taken 35232 times.
|
88684 | if (det > maxDet) { |
| 128 |
1/1✓ Branch 1 taken 53452 times.
|
53452 | Matrix AtAInv(3, 3); |
| 129 |
1/1✓ Branch 1 taken 53452 times.
|
53452 | const bool test = invert(AtA, AtAInv); |
| 130 |
1/2✓ Branch 0 taken 53452 times.
✗ Branch 1 not taken.
|
53452 | if (test) { |
| 131 | 53452 | maxDet = det; | |
| 132 |
2/2✓ Branch 1 taken 53452 times.
✓ Branch 4 taken 53452 times.
|
53452 | Vector x = prod(prod(AtAInv, At), b); |
| 133 |
1/1✓ Branch 1 taken 53452 times.
|
53452 | const double a_b = x[0]; |
| 134 |
1/1✓ Branch 1 taken 53452 times.
|
53452 | const double c_b = x[1]; |
| 135 |
1/1✓ Branch 1 taken 53452 times.
|
53452 | const double d_b = x[2]; |
| 136 | |||
| 137 | // a = a_b*b | ||
| 138 | // c = c_b*b | ||
| 139 | // d = d_b*b | ||
| 140 | // a^2 + b^2 + c^2 = 1 | ||
| 141 | // b^2*(a_b^2 + c_b^2 + 1) = 1 | ||
| 142 | |||
| 143 | 53452 | m_b = 1.0 / std::sqrt(a_b * a_b + c_b * c_b + 1.0); | |
| 144 | 53452 | m_a = a_b * m_b; | |
| 145 | 53452 | m_c = c_b * m_b; | |
| 146 |
3/3✓ Branch 2 taken 53452 times.
✓ Branch 6 taken 53452 times.
✓ Branch 10 taken 53452 times.
|
53452 | m_d = d_b * m_b - m_a * points[0].x() - m_b * points[0].y() - m_c * points[0].z(); |
| 147 | 53452 | foundSolution = true; | |
| 148 | 53452 | } | |
| 149 | 53452 | } | |
| 150 | 88684 | } | |
| 151 | |||
| 152 | { | ||
| 153 | // Ax = b, x = [b/a; c/a; d/a] | ||
| 154 |
1/1✓ Branch 1 taken 88684 times.
|
88684 | Matrix A(N, 3); |
| 155 |
1/1✓ Branch 1 taken 88684 times.
|
88684 | Matrix At(3, N); |
| 156 |
1/1✓ Branch 1 taken 88684 times.
|
88684 | Vector b(N); |
| 157 |
2/2✓ Branch 0 taken 400580 times.
✓ Branch 1 taken 88684 times.
|
489264 | for (unsigned i = 0; i < N; ++i) { |
| 158 |
3/3✓ Branch 2 taken 400580 times.
✓ Branch 6 taken 400580 times.
✓ Branch 9 taken 400580 times.
|
400580 | A(i, 0) = points[i].y() - points[0].y(); |
| 159 |
3/3✓ Branch 2 taken 400580 times.
✓ Branch 6 taken 400580 times.
✓ Branch 9 taken 400580 times.
|
400580 | A(i, 1) = points[i].z() - points[0].z(); |
| 160 |
1/1✓ Branch 1 taken 400580 times.
|
400580 | A(i, 2) = 1.0; |
| 161 |
2/2✓ Branch 1 taken 400580 times.
✓ Branch 4 taken 400580 times.
|
400580 | At(0, i) = A(i, 0); |
| 162 |
2/2✓ Branch 1 taken 400580 times.
✓ Branch 4 taken 400580 times.
|
400580 | At(1, i) = A(i, 1); |
| 163 |
2/2✓ Branch 1 taken 400580 times.
✓ Branch 4 taken 400580 times.
|
400580 | At(2, i) = A(i, 2); |
| 164 |
3/3✓ Branch 2 taken 400580 times.
✓ Branch 6 taken 400580 times.
✓ Branch 9 taken 400580 times.
|
400580 | b[i] = -(points[i].x() - points[0].x()); |
| 165 | } | ||
| 166 | |||
| 167 |
1/1✓ Branch 1 taken 88684 times.
|
88684 | const Matrix AtA = prod(At, A); |
| 168 |
1/1✓ Branch 1 taken 88684 times.
|
88684 | const double det = det3x3(AtA); // always positive for A'*A |
| 169 |
2/2✓ Branch 0 taken 31750 times.
✓ Branch 1 taken 56934 times.
|
88684 | if (det > maxDet) { |
| 170 |
1/1✓ Branch 1 taken 31750 times.
|
31750 | Matrix AtAInv(3, 3); |
| 171 |
1/1✓ Branch 1 taken 31750 times.
|
31750 | const bool test = invert(AtA, AtAInv); |
| 172 |
1/2✓ Branch 0 taken 31750 times.
✗ Branch 1 not taken.
|
31750 | if (test) { |
| 173 |
2/2✓ Branch 1 taken 31750 times.
✓ Branch 4 taken 31750 times.
|
31750 | Vector x = prod(prod(AtAInv, At), b); |
| 174 |
1/1✓ Branch 1 taken 31750 times.
|
31750 | const double b_a = x[0]; |
| 175 |
1/1✓ Branch 1 taken 31750 times.
|
31750 | const double c_a = x[1]; |
| 176 |
1/1✓ Branch 1 taken 31750 times.
|
31750 | const double d_a = x[2]; |
| 177 | |||
| 178 | // b = b_a*a | ||
| 179 | // c = c_a*a | ||
| 180 | // d = d_a*a | ||
| 181 | // a^2 + b^2 + c^2 = 1 | ||
| 182 | // a^2*(b_a^2 + c_a^2 + 1) = 1 | ||
| 183 | |||
| 184 | 31750 | m_a = 1.0 / std::sqrt(b_a * b_a + c_a * c_a + 1.0); | |
| 185 | 31750 | m_b = b_a * m_a; | |
| 186 | 31750 | m_c = c_a * m_a; | |
| 187 |
3/3✓ Branch 2 taken 31750 times.
✓ Branch 6 taken 31750 times.
✓ Branch 10 taken 31750 times.
|
31750 | m_d = d_a * m_a - m_a * points[0].x() - m_b * points[0].y() - m_c * points[0].z(); |
| 188 | 31750 | foundSolution = true; | |
| 189 | 31750 | } | |
| 190 | 31750 | } | |
| 191 | 88684 | } | |
| 192 | |||
| 193 |
1/2✗ Branch 0 not taken.
✓ Branch 1 taken 88684 times.
|
88684 | if (!foundSolution) { |
| 194 | //std::stringstream ss; | ||
| 195 | //ss << std::fixed << std::setprecision(20) << points << '\n'; | ||
| 196 | //std::string s = ss.str(); | ||
| 197 | ✗ | LOG_AND_THROW("Cannot compute plane for points " << points); | |
| 198 | } | ||
| 199 | |||
| 200 | // get outward normal from vertices, plane outward normal should match sense of vertices | ||
| 201 | // this corresponds to the other solution to the sqrt | ||
| 202 |
1/1✓ Branch 1 taken 88684 times.
|
88684 | boost::optional<Vector3d> outwardNormal = getOutwardNormal(points); |
| 203 |
2/2✓ Branch 0 taken 88683 times.
✓ Branch 1 taken 1 time.
|
88684 | if (outwardNormal) { |
| 204 |
6/6✓ Branch 1 taken 88683 times.
✓ Branch 4 taken 88683 times.
✓ Branch 7 taken 88683 times.
✓ Branch 10 taken 88683 times.
✓ Branch 13 taken 88683 times.
✓ Branch 16 taken 88683 times.
|
88683 | const double thisDot = m_a * outwardNormal.get().x() + m_b * outwardNormal.get().y() + m_c * outwardNormal.get().z(); |
| 205 |
2/2✓ Branch 0 taken 43802 times.
✓ Branch 1 taken 44881 times.
|
88683 | if (thisDot < 0) { |
| 206 | 43802 | m_a = -m_a; | |
| 207 | 43802 | m_b = -m_b; | |
| 208 | 43802 | m_c = -m_c; | |
| 209 | 43802 | m_d = -m_d; | |
| 210 | } | ||
| 211 | } | ||
| 212 | |||
| 213 | // test that normal has length 1 | ||
| 214 | 88684 | const double length = m_a * m_a + m_b * m_b + m_c * m_c; | |
| 215 | 88684 | constexpr double tol = 0.0001; | |
| 216 |
1/3✗ Branch 0 not taken.
✓ Branch 1 taken 88684 times.
✗ Branch 3 not taken.
|
88684 | OS_ASSERT(std::fabs(1.0 - length) <= tol); |
| 217 | 88684 | } | |
| 218 | 90539 | } | |
| 219 | |||
| 220 | 4 | Plane::Plane(double a, double b, double c, double d) : m_a(a), m_b(b), m_c(c), m_d(d) { | |
| 221 | // test that normal has length 1 | ||
| 222 | 4 | const double length = m_a * m_a + m_b * m_b + m_c * m_c; | |
| 223 | 4 | constexpr double tol = 0.0001; | |
| 224 |
1/2✗ Branch 0 not taken.
✓ Branch 1 taken 4 times.
|
4 | OS_ASSERT(std::fabs(1.0 - length) <= tol); |
| 225 | 4 | } | |
| 226 | |||
| 227 | 1043200 | Vector3d Plane::outwardNormal() const { | |
| 228 | 1043200 | return {m_a, m_b, m_c}; | |
| 229 | } | ||
| 230 | |||
| 231 | 19 | bool Plane::parallel(const Plane& other, double tol) const { | |
| 232 | // dot product of outward normals should be 1 or negative 1 | ||
| 233 | 19 | const double thisDot = (m_a * other.a() + m_b * other.b() + m_c * other.c()); | |
| 234 | 19 | const bool result = (std::fabs(thisDot) >= 1.0 - tol); | |
| 235 | 19 | return result; | |
| 236 | } | ||
| 237 | |||
| 238 | // is this plane equal to the other plane | ||
| 239 | // true if dot product of outward normals is >= (1-tol) and std::abs(d1-d2) <= tol | ||
| 240 | 283 | bool Plane::equal(const Plane& other, double tol) const { | |
| 241 | 283 | const double thisDot = (m_a * other.a() + m_b * other.b() + m_c * other.c()); | |
| 242 | 283 | const double dist = m_d - other.d(); | |
| 243 |
4/4✓ Branch 0 taken 47 times.
✓ Branch 1 taken 236 times.
✓ Branch 2 taken 41 times.
✓ Branch 3 taken 6 times.
|
283 | const bool result = (thisDot >= 1 - tol) && (std::fabs(dist) <= tol); |
| 244 | 283 | return result; | |
| 245 | } | ||
| 246 | |||
| 247 | // is this plane reverse equal to the other plane | ||
| 248 | // true if dot product of outward normals is <= (-1+tol) and std::abs(d1+d2) <= tol | ||
| 249 | 522213 | bool Plane::reverseEqual(const Plane& other, double tol) const { | |
| 250 | 522213 | const double thisDot = (m_a * other.a() + m_b * other.b() + m_c * other.c()); | |
| 251 | 522213 | const double dist = m_d + other.d(); | |
| 252 |
4/4✓ Branch 0 taken 50301 times.
✓ Branch 1 taken 471912 times.
✓ Branch 2 taken 25537 times.
✓ Branch 3 taken 24764 times.
|
522213 | const bool result = (thisDot <= -1 + tol) && (std::fabs(dist) <= tol); |
| 253 | 522213 | return result; | |
| 254 | } | ||
| 255 | |||
| 256 | 4 | Plane Plane::reversePlane() const { | |
| 257 | 4 | return {-m_a, -m_b, -m_c, -m_d}; | |
| 258 | } | ||
| 259 | |||
| 260 | 1048736 | Point3d Plane::project(const Point3d& point) const { | |
| 261 | // http://www.9math.com/book/projection-point-plane | ||
| 262 | 1048736 | const double u = point.x(); | |
| 263 | 1048736 | const double v = point.y(); | |
| 264 | 1048736 | const double w = point.z(); | |
| 265 | |||
| 266 | 1048736 | const double num = m_a * u + m_b * v + m_c * w + m_d; | |
| 267 | 1048736 | const double den = m_a * m_a + m_b * m_b + m_c * m_c; // this should always be 1.0 | |
| 268 | 1048736 | const double ratio = num / den; | |
| 269 | |||
| 270 | 1048736 | const double x = u - m_a * ratio; | |
| 271 | 1048736 | const double y = v - m_b * ratio; | |
| 272 | 1048736 | const double z = w - m_c * ratio; | |
| 273 | |||
| 274 | 1048736 | return {x, y, z}; | |
| 275 | } | ||
| 276 | |||
| 277 | 1380 | std::vector<Point3d> Plane::project(const std::vector<Point3d>& points) const { | |
| 278 | 1380 | std::vector<Point3d> result; | |
| 279 |
1/1✓ Branch 2 taken 1380 times.
|
1380 | result.reserve(points.size()); |
| 280 |
2/2✓ Branch 4 taken 5520 times.
✓ Branch 5 taken 1380 times.
|
6900 | for (const Point3d& point : points) { |
| 281 |
2/2✓ Branch 1 taken 5520 times.
✓ Branch 4 taken 5520 times.
|
5520 | result.push_back(this->project(point)); |
| 282 | } | ||
| 283 | 1380 | return result; | |
| 284 | ✗ | } | |
| 285 | |||
| 286 | 94 | bool Plane::pointOnPlane(const Point3d& point, double tol) const { | |
| 287 | // project point to plane | ||
| 288 |
1/1✓ Branch 1 taken 94 times.
|
94 | const Point3d projected = project(point); |
| 289 | |||
| 290 | // get distance | ||
| 291 |
1/1✓ Branch 1 taken 94 times.
|
94 | const Vector3d diff = point - projected; |
| 292 |
1/1✓ Branch 1 taken 94 times.
|
94 | const double dist = diff.length(); |
| 293 | |||
| 294 | 94 | return (dist <= tol); | |
| 295 | 94 | } | |
| 296 | |||
| 297 | 522981 | double Plane::a() const { | |
| 298 | 522981 | return m_a; | |
| 299 | } | ||
| 300 | |||
| 301 | 522981 | double Plane::b() const { | |
| 302 | 522981 | return m_b; | |
| 303 | } | ||
| 304 | |||
| 305 | 522981 | double Plane::c() const { | |
| 306 | 522981 | return m_c; | |
| 307 | } | ||
| 308 | |||
| 309 | 522987 | double Plane::d() const { | |
| 310 | 522987 | return m_d; | |
| 311 | } | ||
| 312 | |||
| 313 | 7 | Point3d Plane::anyPointOnPlane() const { | |
| 314 | |||
| 315 |
2/2✓ Branch 1 taken 4 times.
✓ Branch 2 taken 3 times.
|
7 | if (std::abs(m_d) < 0.0001) { |
| 316 | 4 | return {0.0, 0.0, 0.0}; | |
| 317 | } | ||
| 318 | // a*x + b*y + c*z + d = 0, any point that satisfies this equation is on the plane. | ||
| 319 |
2/2✓ Branch 1 taken 1 time.
✓ Branch 2 taken 2 times.
|
3 | if (std::abs(m_a) > 0.0) { |
| 320 | 1 | return {-m_d / m_a, 0.0, 0.0}; | |
| 321 | } | ||
| 322 |
1/2✓ Branch 1 taken 2 times.
✗ Branch 2 not taken.
|
2 | if (std::abs(m_b) > 0.0) { |
| 323 | 2 | return {0.0, -m_d / m_b, 0.0}; | |
| 324 | } | ||
| 325 | |||
| 326 | ✗ | return {0.0, 0.0, -m_d / m_c}; | |
| 327 | } | ||
| 328 | |||
| 329 | 36 | boost::optional<Point3d> Plane::rayIntersection(const Point3d& rayOrigin, const Vector3d& rayDirection, bool enforceDirectionOfPlane) const { | |
| 330 | |||
| 331 | // Any point on plane p = (x, y, z): | ||
| 332 | // a*x + b*y +c*z + d = 0 | ||
| 333 | // <=> Pn · p + d = 0, where Pn is the plane outwardNormal (1) | ||
| 334 | // | ||
| 335 | // Any point on a ray, given it's origin r0 (X0, Y0, Z0) and the direction rD (Xd, Yd, Zd), satisfies this equation | ||
| 336 | // r(t) = r0 + t * rD, **with t > 0** (2) | ||
| 337 | // (t > 0 because, directed ray, otherwise, it's a line) | ||
| 338 | // | ||
| 339 | // If there is an intersection, then this equation must be satisfied: | ||
| 340 | // Pn · r(t) + d = 0 | ||
| 341 | // <=> Pn · R0 + t * (Pn·Rd) + d = 0 | ||
| 342 | // <=> t = -((Pn · R0) + d) / (Pn · Rd), **With t > 0** | ||
| 343 | |||
| 344 |
1/1✓ Branch 1 taken 36 times.
|
36 | const Point3d p0{}; |
| 345 | |||
| 346 |
1/1✓ Branch 1 taken 36 times.
|
36 | auto thisOutwardNormal = outwardNormal(); |
| 347 | |||
| 348 |
1/1✓ Branch 1 taken 36 times.
|
36 | const double vd = thisOutwardNormal.dot(rayDirection); |
| 349 | |||
| 350 |
2/2✓ Branch 1 taken 24 times.
✓ Branch 2 taken 12 times.
|
36 | if (std::abs(vd) < 0.001) { |
| 351 |
5/5✓ Branch 1 taken 24 times.
✓ Branch 4 taken 24 times.
✓ Branch 7 taken 24 times.
✓ Branch 10 taken 24 times.
✓ Branch 13 taken 24 times.
|
24 | LOG(Debug, "Plane is orthogonal to Ray, no intersection possible"); |
| 352 | 24 | return {}; | |
| 353 | } | ||
| 354 |
2/2✓ Branch 0 taken 6 times.
✓ Branch 1 taken 6 times.
|
12 | if (vd > 0) { |
| 355 |
1/2✗ Branch 0 not taken.
✓ Branch 1 taken 6 times.
|
6 | if (enforceDirectionOfPlane) { |
| 356 | ✗ | LOG(Debug, "Plane normal and ray are poiting in the same direction..."); | |
| 357 | ✗ | return {}; | |
| 358 | } else { | ||
| 359 |
5/5✓ Branch 1 taken 6 times.
✓ Branch 4 taken 6 times.
✓ Branch 7 taken 6 times.
✓ Branch 10 taken 6 times.
✓ Branch 13 taken 6 times.
|
6 | LOG(Trace, "Plane normal and ray are poiting in the same direction..."); |
| 360 | } | ||
| 361 | } else { | ||
| 362 |
5/5✓ Branch 1 taken 6 times.
✓ Branch 4 taken 6 times.
✓ Branch 7 taken 6 times.
✓ Branch 10 taken 6 times.
✓ Branch 13 taken 6 times.
|
6 | LOG(Trace, "Plane normal and ray are poiting in the opposite direction..."); |
| 363 | } | ||
| 364 | |||
| 365 |
2/2✓ Branch 1 taken 12 times.
✓ Branch 4 taken 12 times.
|
12 | const double v0 = -(thisOutwardNormal.dot(rayOrigin - p0) + m_d); |
| 366 | 12 | const double t = v0 / vd; | |
| 367 |
10/10✓ Branch 1 taken 12 times.
✓ Branch 4 taken 12 times.
✓ Branch 7 taken 12 times.
✓ Branch 10 taken 12 times.
✓ Branch 13 taken 12 times.
✓ Branch 16 taken 12 times.
✓ Branch 19 taken 12 times.
✓ Branch 22 taken 12 times.
✓ Branch 25 taken 12 times.
✓ Branch 28 taken 12 times.
|
12 | LOG(Trace, "vd=" << vd << ", v0=" << v0 << ", t=" << t); |
| 368 |
2/2✓ Branch 0 taken 6 times.
✓ Branch 1 taken 6 times.
|
12 | if (t > 0.0) { |
| 369 |
3/3✓ Branch 1 taken 6 times.
✓ Branch 4 taken 6 times.
✓ Branch 7 taken 6 times.
|
6 | return rayOrigin + t * rayDirection; |
| 370 | } | ||
| 371 | |||
| 372 | 6 | return {}; | |
| 373 | 36 | } | |
| 374 | |||
| 375 | 459 | std::ostream& operator<<(std::ostream& os, const Plane& plane) { | |
| 376 | // os << fmt::format("Plane: {:g}x {:+g}y {:+g}z {:+g} = 0", plane.a(), plane.b(), plane.c(), plane.d()); | ||
| 377 | 459 | os << "[" << plane.a() << ", " << plane.b() << ", " << plane.c() << ", " << plane.d() << "]"; | |
| 378 | 459 | return os; | |
| 379 | } | ||
| 380 | |||
| 381 | } // namespace openstudio | ||
| 382 |