1 /********************************************************************************
2 * *
3 * S i n g 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,2006 by Jeroen van der Zijp. All Rights Reserved. *
7 *********************************************************************************
8 * This library is free software; you can redistribute it and/or *
9 * modify it under the terms of the GNU Lesser General Public *
10 * License as published by the Free Software Foundation; either *
11 * version 2.1 of the License, or (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 GNU *
16 * Lesser General Public License for more details. *
17 * *
18 * You should have received a copy of the GNU Lesser General Public *
19 * License along with this library; if not, write to the Free Software *
20 * Foundation, Inc., 59 Temple Place, Suite 330, Boston, MA 02111-1307 USA. *
21 *********************************************************************************
22 * $Id: FXSphered.cpp 4937 2019-03-10 19:59:30Z arthurcnorman $ *
23 ********************************************************************************/
24 #include "xincs.h"
25 #include "fxver.h"
26 #include "fxdefs.h"
27 #include "FXHash.h"
28 #include "FXStream.h"
29 #include "FXVec2d.h"
30 #include "FXVec3d.h"
31 #include "FXVec4d.h"
32 #include "FXSphered.h"
33 #include "FXRanged.h"
34
35 /*
36 Notes:
37 - Negative radius represents empty bounding sphere.
38 */
39
40
41 using namespace FX;
42
43 /************************** S p h e r e C l a s s *************************/
44
45 namespace FX {
46
47
sqr(FXdouble x)48 inline FXdouble sqr(FXdouble x){ return x*x; }
49
50
51 // Initialize from bounding box
FXSphered(const FXRanged & bounds)52 FXSphered::FXSphered(const FXRanged& bounds):center(bounds.center()),radius(bounds.diameter()*0.5){
53 }
54
55
56 // Test if sphere contains point x,y,z
contains(FXdouble x,FXdouble y,FXdouble z) const57 bool FXSphered::contains(FXdouble x,FXdouble y,FXdouble z) const {
58 return 0.0<=radius && sqr(center.x-x)+sqr(center.y-y)+sqr(center.z-z)<=sqr(radius);
59 }
60
61
62 // Test if sphere contains point p
contains(const FXVec3d & p) const63 bool FXSphered::contains(const FXVec3d& p) const {
64 return contains(p.x,p.y,p.z);
65 }
66
67
68 // Test if sphere contains another box
contains(const FXRanged & box) const69 bool FXSphered::contains(const FXRanged& box) const {
70 if(box.lower.x<=box.upper.x && box.lower.y<=box.upper.y && box.lower.z<=box.upper.z){
71 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));
72 }
73 return FALSE;
74 }
75
76
77 // Test if sphere properly contains another sphere
contains(const FXSphered & sphere) const78 bool FXSphered::contains(const FXSphered& sphere) const {
79 if(0.0<=sphere.radius && sphere.radius<=radius){
80 FXdouble dx=center.x-sphere.center.x;
81 FXdouble dy=center.y-sphere.center.y;
82 FXdouble dz=center.z-sphere.center.z;
83 return sphere.radius+sqrt(dx*dx+dy*dy+dz*dz)<=radius;
84 }
85 return FALSE;
86 }
87
88
89 // Include point
include(FXdouble x,FXdouble y,FXdouble z)90 FXSphered& FXSphered::include(FXdouble x,FXdouble y,FXdouble z){
91 FXdouble dx,dy,dz,dist,delta,newradius;
92 if(0.0<=radius){
93 dx=x-center.x;
94 dy=y-center.y;
95 dz=z-center.z;
96 dist=sqrt(dx*dx+dy*dy+dz*dz);
97 if(radius<dist){
98 newradius=0.5*(radius+dist);
99 delta=(newradius-radius);
100 center.x+=delta*dx/dist;
101 center.y+=delta*dy/dist;
102 center.z+=delta*dz/dist;
103 radius=newradius;
104 }
105 return *this;
106 }
107 center.x=x;
108 center.y=y;
109 center.z=z;
110 radius=0.0;
111 return *this;
112 }
113
114
115 // Include point
include(const FXVec3d & p)116 FXSphered& FXSphered::include(const FXVec3d& p){
117 return include(p.x,p.y,p.z);
118 }
119
120
121 // Expand radius to include point
includeInRadius(FXdouble x,FXdouble y,FXdouble z)122 FXSphered& FXSphered::includeInRadius(FXdouble x,FXdouble y,FXdouble z){
123 FXdouble dx,dy,dz,dist;
124 if(0.0<=radius){
125 dx=x-center.x;
126 dy=y-center.y;
127 dz=z-center.z;
128 dist=sqrt(dx*dx+dy*dy+dz*dz);
129 if(radius<dist) radius=dist;
130 return *this;
131 }
132 center.x=x;
133 center.y=y;
134 center.z=z;
135 radius=0.0;
136 return *this;
137 }
138
139
140 // Expand radius to include point
includeInRadius(const FXVec3d & p)141 FXSphered& FXSphered::includeInRadius(const FXVec3d& p){
142 return includeInRadius(p.x,p.y,p.z);
143 }
144
145
146 // Include given range into this one
include(const FXRanged & box)147 FXSphered& FXSphered::include(const FXRanged& box){
148 if(box.lower.x<=box.upper.x && box.lower.y<=box.upper.y && box.lower.z<=box.upper.z){
149 if(0.0<=radius){
150 include(box.corner(0));
151 include(box.corner(1));
152 include(box.corner(2));
153 include(box.corner(3));
154 include(box.corner(4));
155 include(box.corner(5));
156 include(box.corner(6));
157 include(box.corner(7));
158 return *this;
159 }
160 center=box.center();
161 radius=box.radius();
162 }
163 return *this;
164 }
165
166
167 // Expand radius to include box
includeInRadius(const FXRanged & box)168 FXSphered& FXSphered::includeInRadius(const FXRanged& box){
169 if(box.lower.x<=box.upper.x && box.lower.y<=box.upper.y && box.lower.z<=box.upper.z){
170 if(0.0<=radius){
171 includeInRadius(box.corner(0));
172 includeInRadius(box.corner(1));
173 includeInRadius(box.corner(2));
174 includeInRadius(box.corner(3));
175 includeInRadius(box.corner(4));
176 includeInRadius(box.corner(5));
177 includeInRadius(box.corner(6));
178 includeInRadius(box.corner(7));
179 return *this;
180 }
181 center=box.center();
182 radius=box.radius();
183 }
184 return *this;
185 }
186
187
188 // Include given sphere into this one
include(const FXSphered & sphere)189 FXSphered& FXSphered::include(const FXSphered& sphere){
190 FXdouble dx,dy,dz,dist,delta,newradius;
191 if(0.0<=sphere.radius){
192 if(0.0<=radius){
193 dx=sphere.center.x-center.x;
194 dy=sphere.center.y-center.y;
195 dz=sphere.center.z-center.z;
196 dist=sqrt(dx*dx+dy*dy+dz*dz);
197 if(sphere.radius<dist+radius){
198 if(radius<dist+sphere.radius){
199 newradius=0.5*(radius+dist+sphere.radius);
200 delta=(newradius-radius);
201 center.x+=delta*dx/dist;
202 center.y+=delta*dy/dist;
203 center.z+=delta*dz/dist;
204 radius=newradius;
205 }
206 return *this;
207 }
208 }
209 center=sphere.center;
210 radius=sphere.radius;
211 }
212 return *this;
213 }
214
215
216 // Expand radius to include sphere
includeInRadius(const FXSphered & sphere)217 FXSphered& FXSphered::includeInRadius(const FXSphered& sphere){
218 FXdouble dx,dy,dz,dist;
219 if(0.0<=sphere.radius){
220 if(0.0<=radius){
221 dx=sphere.center.x-center.x;
222 dy=sphere.center.y-center.y;
223 dz=sphere.center.z-center.z;
224 dist=sqrt(dx*dx+dy*dy+dz*dz)+sphere.radius;
225 if(radius<dist) radius=dist;
226 return *this;
227 }
228 center=sphere.center;
229 radius=sphere.radius;
230 }
231 return *this;
232 }
233
234
235 // Intersect sphere with normalized plane ax+by+cz+w; returns -1,0,+1
intersect(const FXVec4d & plane) const236 FXint FXSphered::intersect(const FXVec4d& plane) const {
237 FXdouble dist=plane.distance(center);
238
239 // Lower point on positive side of plane
240 if(dist>=radius) return 1;
241
242 // Upper point on negative side of plane
243 if(dist<=-radius) return -1;
244
245 // Overlap
246 return 0;
247 }
248
249
250 // Intersect sphere with ray u-v
intersect(const FXVec3d & u,const FXVec3d & v) const251 bool FXSphered::intersect(const FXVec3d& u,const FXVec3d& v) const {
252 if(0.0<=radius){
253 FXdouble rr=radius*radius;
254 FXVec3d uc=center-u; // Vector from u to center
255 FXdouble dd=uc.length2();
256 if(dd>rr){ // Ray start point outside sphere
257 FXVec3d uv=v-u; // Vector from u to v
258 FXdouble hh=uc*uv; // If hh<0, uv points away from center
259 if(0.0<=hh){ // Not away from sphere
260 FXdouble kk=uv.length2();
261 FXdouble disc=hh*hh-kk*(dd-rr); // FIXME this needs to be checked again!
262 if(disc<=0.0) return FALSE;
263 return TRUE;
264 }
265 return FALSE;
266 }
267 return TRUE;
268 }
269 return FALSE;
270 }
271
272
273 // Test if sphere overlaps with box
overlap(const FXSphered & a,const FXRanged & b)274 bool overlap(const FXSphered& a,const FXRanged& b){
275 if(0.0<=a.radius){
276 FXdouble dd=0.0;
277
278 if(a.center.x<b.lower.x)
279 dd+=sqr(a.center.x-b.lower.x);
280 else if(a.center.x>b.upper.x)
281 dd+=sqr(a.center.x-b.upper.x);
282
283 if(a.center.y<b.lower.y)
284 dd+=sqr(a.center.y-b.lower.y);
285 else if(a.center.y>b.upper.y)
286 dd+=sqr(a.center.y-b.upper.y);
287
288 if(a.center.z<b.lower.z)
289 dd+=sqr(a.center.z-b.lower.z);
290 else if(a.center.z>b.upper.z)
291 dd+=sqr(a.center.z-b.upper.z);
292
293 return dd<=a.radius*a.radius;
294 }
295 return FALSE;
296 }
297
298
299 // Test if box overlaps with sphere; algorithm due to Arvo (GEMS I)
overlap(const FXRanged & a,const FXSphered & b)300 bool overlap(const FXRanged& a,const FXSphered& b){
301 return overlap(b,a);
302 }
303
304
305 // Test if spheres overlap
overlap(const FXSphered & a,const FXSphered & b)306 bool overlap(const FXSphered& a,const FXSphered& b){
307 if(0.0<=a.radius && 0.0<=b.radius){
308 FXdouble dx=a.center.x-b.center.x;
309 FXdouble dy=a.center.y-b.center.y;
310 FXdouble dz=a.center.z-b.center.z;
311 return (dx*dx+dy*dy+dz*dz)<sqr(a.radius+b.radius);
312 }
313 return FALSE;
314 }
315
316
317 // Saving
operator <<(FXStream & store,const FXSphered & sphere)318 FXStream& operator<<(FXStream& store,const FXSphered& sphere){
319 store << sphere.center << sphere.radius;
320 return store;
321 }
322
323
324 // Loading
operator >>(FXStream & store,FXSphered & sphere)325 FXStream& operator>>(FXStream& store,FXSphered& sphere){
326 store >> sphere.center >> sphere.radius;
327 return store;
328 }
329
330 }
331