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