1 /********************************************************************************
2 * *
3 * D o u b l e - P r e c i s i o n S p h e r e C l a s s *
4 * *
5 *********************************************************************************
6 * Copyright (C) 2004,2020 by Jeroen van der Zijp. All Rights Reserved. *
7 *********************************************************************************
8 * This library is free software; you can redistribute it and/or modify *
9 * it under the terms of the GNU Lesser General Public License as published by *
10 * the Free Software Foundation; either version 3 of the License, or *
11 * (at your option) any later version. *
12 * *
13 * This library is distributed in the hope that it will be useful, *
14 * but WITHOUT ANY WARRANTY; without even the implied warranty of *
15 * MERCHANTABILITY or FITNESS FOR A PARTICULAR PURPOSE. See the *
16 * GNU Lesser General Public License for more details. *
17 * *
18 * You should have received a copy of the GNU Lesser General Public License *
19 * along with this program. If not, see <http://www.gnu.org/licenses/> *
20 ********************************************************************************/
21 #include "xincs.h"
22 #include "fxver.h"
23 #include "fxdefs.h"
24 #include "fxmath.h"
25 #include "FXArray.h"
26 #include "FXHash.h"
27 #include "FXStream.h"
28 #include "FXVec2d.h"
29 #include "FXVec3d.h"
30 #include "FXVec4d.h"
31 #include "FXSphered.h"
32 #include "FXRanged.h"
33 #include "FXMat3d.h"
34 #include "FXMat4d.h"
35
36 /*
37 Notes:
38 - Negative radius represents empty bounding sphere.
39 */
40
41
42 using namespace FX;
43
44 /************************** S p h e r e C l a s s *************************/
45
46 namespace FX {
47
48
49 // Initialize from bounding box
FXSphered(const FXRanged & bounds)50 FXSphered::FXSphered(const FXRanged& bounds):center(bounds.center()),radius(bounds.diameter()*0.5){
51 }
52
53
54 // Test if sphere contains point x,y,z
contains(FXdouble x,FXdouble y,FXdouble z) const55 FXbool FXSphered::contains(FXdouble x,FXdouble y,FXdouble z) const {
56 return 0.0<=radius && Math::sqr(center.x-x)+Math::sqr(center.y-y)+Math::sqr(center.z-z)<=Math::sqr(radius);
57 }
58
59
60 // Test if sphere contains point p
contains(const FXVec3d & p) const61 FXbool FXSphered::contains(const FXVec3d& p) const {
62 return contains(p.x,p.y,p.z);
63 }
64
65
66 // Test if sphere contains another box
contains(const FXRanged & box) const67 FXbool FXSphered::contains(const FXRanged& box) const {
68 if(box.lower.x<=box.upper.x && box.lower.y<=box.upper.y && box.lower.z<=box.upper.z){
69 return contains(box.corner(0)) && contains(box.corner(1)) && contains(box.corner(2)) && contains(box.corner(3)) && contains(box.corner(4)) && contains(box.corner(5)) && contains(box.corner(6)) && contains(box.corner(7));
70 }
71 return false;
72 }
73
74
75 // Test if sphere properly contains another sphere
contains(const FXSphered & sphere) const76 FXbool FXSphered::contains(const FXSphered& sphere) const {
77 if(0.0<=sphere.radius && sphere.radius<=radius){
78 FXdouble dx=center.x-sphere.center.x;
79 FXdouble dy=center.y-sphere.center.y;
80 FXdouble dz=center.z-sphere.center.z;
81 return sphere.radius+Math::sqrt(dx*dx+dy*dy+dz*dz)<=radius;
82 }
83 return false;
84 }
85
86
87 // Include point
include(FXdouble x,FXdouble y,FXdouble z)88 FXSphered& FXSphered::include(FXdouble x,FXdouble y,FXdouble z){
89 if(0.0<=radius){
90 FXdouble dx=x-center.x;
91 FXdouble dy=y-center.y;
92 FXdouble dz=z-center.z;
93 FXdouble dist=Math::sqrt(dx*dx+dy*dy+dz*dz);
94 if(radius<dist){
95 FXdouble newradius=0.5*(radius+dist);
96 FXdouble delta=(newradius-radius);
97 center.x+=delta*dx/dist;
98 center.y+=delta*dy/dist;
99 center.z+=delta*dz/dist;
100 radius=newradius;
101 }
102 return *this;
103 }
104 center.x=x;
105 center.y=y;
106 center.z=z;
107 radius=0.0;
108 return *this;
109 }
110
111
112 // Include point
include(const FXVec3d & p)113 FXSphered& FXSphered::include(const FXVec3d& p){
114 return include(p.x,p.y,p.z);
115 }
116
117
118 // Expand radius to include point
includeInRadius(FXdouble x,FXdouble y,FXdouble z)119 FXSphered& FXSphered::includeInRadius(FXdouble x,FXdouble y,FXdouble z){
120 if(0.0<=radius){
121 FXdouble dx=x-center.x;
122 FXdouble dy=y-center.y;
123 FXdouble dz=z-center.z;
124 FXdouble dist=Math::sqrt(dx*dx+dy*dy+dz*dz);
125 if(radius<dist) radius=dist;
126 return *this;
127 }
128 center.x=x;
129 center.y=y;
130 center.z=z;
131 radius=0.0;
132 return *this;
133 }
134
135
136 // Expand radius to include point
includeInRadius(const FXVec3d & p)137 FXSphered& FXSphered::includeInRadius(const FXVec3d& p){
138 return includeInRadius(p.x,p.y,p.z);
139 }
140
141
142 // Include given range into this one
include(const FXRanged & box)143 FXSphered& FXSphered::include(const FXRanged& box){
144 if(box.lower.x<=box.upper.x && box.lower.y<=box.upper.y && box.lower.z<=box.upper.z){
145 if(0.0<=radius){
146 include(box.corner(0));
147 include(box.corner(7));
148 include(box.corner(1));
149 include(box.corner(6));
150 include(box.corner(2));
151 include(box.corner(5));
152 include(box.corner(3));
153 include(box.corner(4));
154 return *this;
155 }
156 center=box.center();
157 radius=box.radius();
158 }
159 return *this;
160 }
161
162
163 // Expand radius to include box
includeInRadius(const FXRanged & box)164 FXSphered& FXSphered::includeInRadius(const FXRanged& box){
165 if(box.lower.x<=box.upper.x && box.lower.y<=box.upper.y && box.lower.z<=box.upper.z){
166 if(0.0<=radius){
167 includeInRadius(box.corner(0));
168 includeInRadius(box.corner(7));
169 includeInRadius(box.corner(1));
170 includeInRadius(box.corner(6));
171 includeInRadius(box.corner(2));
172 includeInRadius(box.corner(5));
173 includeInRadius(box.corner(3));
174 includeInRadius(box.corner(4));
175 return *this;
176 }
177 center=box.center();
178 radius=box.radius();
179 }
180 return *this;
181 }
182
183
184 // Include given sphere into this one
include(const FXSphered & sphere)185 FXSphered& FXSphered::include(const FXSphered& sphere){
186 if(0.0<=sphere.radius){
187 if(0.0<=radius){
188 FXdouble dx=sphere.center.x-center.x;
189 FXdouble dy=sphere.center.y-center.y;
190 FXdouble dz=sphere.center.z-center.z;
191 FXdouble dist=Math::sqrt(dx*dx+dy*dy+dz*dz);
192 if(sphere.radius<dist+radius){
193 if(radius<dist+sphere.radius){
194 FXdouble newradius=0.5*(radius+dist+sphere.radius);
195 FXdouble delta=(newradius-radius);
196 center.x+=delta*dx/dist;
197 center.y+=delta*dy/dist;
198 center.z+=delta*dz/dist;
199 radius=newradius;
200 }
201 return *this;
202 }
203 }
204 center=sphere.center;
205 radius=sphere.radius;
206 }
207 return *this;
208 }
209
210
211 // Expand radius to include sphere
includeInRadius(const FXSphered & sphere)212 FXSphered& FXSphered::includeInRadius(const FXSphered& sphere){
213 if(0.0<=sphere.radius){
214 if(0.0<=radius){
215 FXdouble dx=sphere.center.x-center.x;
216 FXdouble dy=sphere.center.y-center.y;
217 FXdouble dz=sphere.center.z-center.z;
218 FXdouble dist=Math::sqrt(dx*dx+dy*dy+dz*dz)+sphere.radius;
219 if(radius<dist) radius=dist;
220 return *this;
221 }
222 center=sphere.center;
223 radius=sphere.radius;
224 }
225 return *this;
226 }
227
228
229 // Intersect sphere with normalized plane ax+by+cz+w; returns -1,0,+1
intersect(const FXVec4d & plane) const230 FXint FXSphered::intersect(const FXVec4d& plane) const {
231 FXdouble dist=plane.distance(center);
232
233 // Upper point on negative side of plane
234 if(dist<=-radius) return -1;
235
236 // Lower point on positive side of plane
237 if(dist>=radius) return 1;
238
239 // Overlap
240 return 0;
241 }
242
243
244 // Intersect sphere with ray u-v
intersect(const FXVec3d & u,const FXVec3d & v) const245 FXbool FXSphered::intersect(const FXVec3d& u,const FXVec3d& v) const {
246 FXdouble hits[2];
247 return intersect(u,v-u,hits) && 0.0<=hits[1] && hits[0]<=1.0;
248 }
249
250
251 // Intersect box with ray pos+lambda*dir, returning true if hit
intersect(const FXVec3d & pos,const FXVec3d & dir,FXdouble hit[]) const252 FXbool FXSphered::intersect(const FXVec3d& pos,const FXVec3d& dir,FXdouble hit[]) const {
253 if(0.0<=radius){
254 FXVec3d m=center-pos;
255 FXdouble m2=m.length2();
256 FXdouble d2=dir.length2();
257 FXdouble r2=radius*radius;
258 FXdouble b=dir*m;
259 FXdouble disc=b*b-d2*(m2-r2);
260 if(0.0<=disc){
261 disc=Math::sqrt(disc);
262 hit[0]=(-b-disc)/d2;
263 hit[1]=(-b+disc)/d2;
264 return true;
265 }
266 }
267 return false;
268 }
269
270
271 // Test if box overlaps with sphere (QRI Larsson, Moeller, Lengyel)
overlap(const FXSphered & a,const FXRanged & b)272 FXbool overlap(const FXSphered& a,const FXRanged& b){
273 if(0.0<=a.radius){
274 FXdouble e1,e2,e3;
275 if((e1=Math::fmax(b.lower.x-a.center.x,0.0)+Math::fmax(a.center.x-b.upper.x,0.0))>a.radius) return false;
276 if((e2=Math::fmax(b.lower.y-a.center.y,0.0)+Math::fmax(a.center.y-b.upper.y,0.0))>a.radius) return false;
277 if((e3=Math::fmax(b.lower.z-a.center.z,0.0)+Math::fmax(a.center.z-b.upper.z,0.0))>a.radius) return false;
278 return e1*e1+e2*e2+e3*e3<=a.radius*a.radius;
279 }
280 return false;
281 }
282
283
284 // Test if box overlaps with sphere; algorithm due to Arvo (GEMS I)
overlap(const FXRanged & a,const FXSphered & b)285 FXbool overlap(const FXRanged& a,const FXSphered& b){
286 return overlap(b,a);
287 }
288
289
290 // Test if spheres overlap
overlap(const FXSphered & a,const FXSphered & b)291 FXbool overlap(const FXSphered& a,const FXSphered& b){
292 if(0.0<=a.radius && 0.0<=b.radius){
293 FXdouble dx=a.center.x-b.center.x;
294 FXdouble dy=a.center.y-b.center.y;
295 FXdouble dz=a.center.z-b.center.z;
296 return (dx*dx+dy*dy+dz*dz)<Math::sqr(a.radius+b.radius);
297 }
298 return false;
299 }
300
301
302 // Transform sphere by 4x4 matrix
transform(const FXMat4d & mat) const303 FXSphered FXSphered::transform(const FXMat4d& mat) const {
304 if(!empty()){
305 FXdouble xd=mat[0][0]*mat[0][0]+mat[0][1]*mat[0][1]+mat[0][2]*mat[0][2];
306 FXdouble yd=mat[1][0]*mat[1][0]+mat[1][1]*mat[1][1]+mat[1][2]*mat[1][2];
307 FXdouble zd=mat[2][0]*mat[2][0]+mat[2][1]*mat[2][1]+mat[2][2]*mat[2][2];
308 return FXSphered(center*mat,radius*Math::sqrt(Math::fmax(Math::fmax(xd,yd),zd)));
309 }
310 return FXSphered(center*mat,radius);
311 }
312
313
314 // Saving
operator <<(FXStream & store,const FXSphered & sphere)315 FXStream& operator<<(FXStream& store,const FXSphered& sphere){
316 store << sphere.center << sphere.radius;
317 return store;
318 }
319
320
321 // Loading
operator >>(FXStream & store,FXSphered & sphere)322 FXStream& operator>>(FXStream& store,FXSphered& sphere){
323 store >> sphere.center >> sphere.radius;
324 return store;
325 }
326
327 }
328