source: src/lib/math/isosurf/shape.cc@ f27baf

Last change on this file since f27baf was 5d30c1, checked in by Frederik Heber <heber@…>, 13 years ago

Initial commit based on 3.0.0alpha (here claimed as 2.4).

  • simply added all files.
  • Property mode set to 100644
File size: 32.1 KB
Line 
1//
2// shape.cc
3//
4// Copyright (C) 1996 Limit Point Systems, Inc.
5//
6// Author: Curtis Janssen <cljanss@limitpt.com>
7// Maintainer: LPS
8//
9// This file is part of the SC Toolkit.
10//
11// The SC Toolkit is free software; you can redistribute it and/or modify
12// it under the terms of the GNU Library General Public License as published by
13// the Free Software Foundation; either version 2, or (at your option)
14// any later version.
15//
16// The SC Toolkit is distributed in the hope that it will be useful,
17// but WITHOUT ANY WARRANTY; without even the implied warranty of
18// MERCHANTABILITY or FITNESS FOR A PARTICULAR PURPOSE. See the
19// GNU Library General Public License for more details.
20//
21// You should have received a copy of the GNU Library General Public License
22// along with the SC Toolkit; see the file COPYING.LIB. If not, write to
23// the Free Software Foundation, 675 Mass Ave, Cambridge, MA 02139, USA.
24//
25// The U.S. Government is granted a limited license as per AL 91-7.
26//
27
28#ifdef __GNUC__
29#pragma implementation
30#endif
31
32#include <stdio.h>
33#include <util/misc/math.h>
34
35#include <util/misc/formio.h>
36#include <util/keyval/keyval.h>
37#include <math/isosurf/shape.h>
38
39using namespace std;
40using namespace sc;
41
42static const double shape_infinity = 1.0e23;
43
44// given a vector X find which of the points in the vector of
45// vectors, A, is closest to it and return the distance
46static double
47closest_distance(SCVector3& X,SCVector3*A,int n,SCVector3*grad)
48{
49 SCVector3 T = X-A[0];
50 double min = T.dot(T);
51 int imin = 0;
52 for (int i=1; i<n; i++) {
53 T = X-A[i];
54 double tmp = T.dot(T);
55 if (tmp < min) {min = tmp; imin = i;}
56 }
57 if (grad) {
58 T = X - A[imin];
59 T.normalize();
60 *grad = T;
61 }
62 return sqrt(min);
63}
64
65//////////////////////////////////////////////////////////////////////
66// Shape
67
68static ClassDesc Shape_cd(
69 typeid(Shape),"Shape",1,"public Volume",
70 0, 0, 0);
71
72Shape::Shape():
73 Volume()
74{
75}
76
77Shape::Shape(const Ref<KeyVal>& keyval):
78 Volume(keyval)
79{
80}
81
82Shape::~Shape()
83{
84}
85
86void
87Shape::compute()
88{
89 SCVector3 r;
90 get_x(r);
91 if (gradient_needed()) {
92 if (!gradient_implemented()) {
93 ExEnv::errn() << "Shape::compute: gradient not implemented" << endl;
94 abort();
95 }
96 SCVector3 v;
97 set_value(distance_to_surface(r,&v));
98 set_actual_value_accuracy(desired_value_accuracy());
99 set_gradient(v);
100 set_actual_gradient_accuracy(desired_gradient_accuracy());
101 }
102 else if (value_needed()) {
103 set_value(distance_to_surface(r));
104 set_actual_value_accuracy(desired_value_accuracy());
105 }
106 if (hessian_needed()) {
107 ExEnv::errn() << "Shape::compute(): can't do hessian yet" << endl;
108 abort();
109 }
110}
111
112int
113Shape::is_outside(const SCVector3&r) const
114{
115 if (distance_to_surface(r)>0.0) return 1;
116 return 0;
117}
118
119// Shape overrides volume's interpolate so it always gets points on
120// the outside of the shape are always returned.
121
122// interpolate using the bisection algorithm
123void
124Shape::interpolate(const SCVector3& A,
125 const SCVector3& B,
126 double val,
127 SCVector3& result)
128{
129 if (val < 0.0) {
130 failure("Shape::interpolate(): val is < 0.0");
131 }
132
133 set_x(A);
134 double value0 = value() - val;
135
136 set_x(B);
137 double value1 = value() - val;
138
139 if (value0*value1 > 0.0) {
140 failure("Shape::interpolate(): values at endpoints don't bracket val");
141 }
142 else if (value0 == 0.0) {
143 result = A;
144 return;
145 }
146 else if (value1 == 0.0) {
147 result = B;
148 return;
149 }
150
151 SCVector3 BA = B - A;
152
153 double length = BA.norm();
154 int niter = (int) (log(length/interpolation_accuracy())/M_LN2);
155
156 double f0 = 0.0;
157 double f1 = 1.0;
158 double fnext = 0.5;
159
160 SCVector3 X = A + fnext*BA;
161 set_x(X);
162 double valuenext = value() - val;
163
164 do {
165 for (int i=0; i<niter; i++) {
166 if (valuenext*value0 <= 0.0) {
167 value1 = valuenext;
168 f1 = fnext;
169 fnext = (f0 + fnext)*0.5;
170 }
171 else {
172 value0 = valuenext;
173 f0 = fnext;
174 fnext = (fnext + f1)*0.5;
175 }
176 X = A + fnext*BA;
177 set_x(X);
178 valuenext = value() - val;
179 }
180 niter = 1;
181 } while (valuenext < 0.0);
182
183 result = X;
184}
185
186int
187Shape::value_implemented() const
188{
189 return 1;
190}
191
192//////////////////////////////////////////////////////////////////////
193// SphereShape
194
195static ClassDesc SphereShape_cd(
196 typeid(SphereShape),"SphereShape",1,"public Shape",
197 0, create<SphereShape>, 0);
198
199SphereShape::SphereShape(const SCVector3&o,double r):
200 _origin(o),
201 _radius(r)
202{
203}
204
205SphereShape::SphereShape(const SphereShape&s):
206 _origin(s._origin),
207 _radius(s._radius)
208{
209}
210
211SphereShape::SphereShape(const Ref<KeyVal>& keyval):
212 _origin(new PrefixKeyVal(keyval,"origin")),
213 _radius(keyval->doublevalue("radius"))
214{
215}
216
217SphereShape::~SphereShape()
218{
219}
220
221double
222SphereShape::distance_to_surface(const SCVector3&p,SCVector3*grad) const
223{
224 int i;
225 double r2 = 0.0;
226 for (i=0; i<3; i++) {
227 double tmp = p[i] - _origin[i];
228 r2 += tmp*tmp;
229 }
230 double r = sqrt(r2);
231 double d = r - _radius;
232 if (grad) {
233 SCVector3 pv(p);
234 SCVector3 o(_origin);
235 SCVector3 unit = pv - o;
236 unit.normalize();
237 for (i=0; i<3; i++) grad->elem(i) = unit[i];
238 }
239 return d;
240}
241
242void SphereShape::print(ostream&o) const
243{
244 o << indent
245 << scprintf("SphereShape: r = %8.4f o = (%8.4f %8.4f %8.4f)",
246 radius(),origin()[0],origin()[1],origin()[2])
247 << endl;
248}
249
250void
251SphereShape::boundingbox(double valuemin, double valuemax,
252 SCVector3& p1,
253 SCVector3& p2)
254{
255 if (valuemax < 0.0) valuemax = 0.0;
256
257 int i;
258 for (i=0; i<3; i++) {
259 p1[i] = _origin[i] - _radius - valuemax;
260 p2[i] = _origin[i] + _radius + valuemax;
261 }
262}
263
264int
265SphereShape::gradient_implemented() const
266{
267 return 1;
268}
269
270////////////////////////////////////////////////////////////////////////
271// UncappedTorusHoleShape
272
273static ClassDesc UncappedTorusHoleShape_cd(
274 typeid(UncappedTorusHoleShape),"UncappedTorusHoleShape",1,"public Shape",
275 0, 0, 0);
276
277UncappedTorusHoleShape::UncappedTorusHoleShape(double r,
278 const SphereShape& s1,
279 const SphereShape& s2):
280_s1(s1),
281_s2(s2),
282_r(r)
283{
284}
285
286UncappedTorusHoleShape*
287UncappedTorusHoleShape::newUncappedTorusHoleShape(double r,
288 const SphereShape&s1,
289 const SphereShape&s2)
290{
291 // if the probe sphere fits between the two spheres, then there
292 // is no need to include this shape
293 SCVector3 A(s1.origin());
294 SCVector3 B(s2.origin());
295 SCVector3 BA = B - A;
296 if (2.0*r < BA.norm() - s1.radius() - s2.radius()) return 0;
297
298 // check to see if the surface is reentrant
299 double rrs1 = r+s1.radius();
300 double rrs2 = r+s2.radius();
301 SCVector3 R12 = ((SCVector3)s1.origin()) - ((SCVector3)s2.origin());
302 double r12 = sqrt(R12.dot(R12));
303 if (sqrt(rrs1*rrs1-r*r) + sqrt(rrs2*rrs2-r*r) < r12)
304 return new ReentrantUncappedTorusHoleShape(r,s1,s2);
305
306 // otherwise create an ordinary torus hole
307 return new NonreentrantUncappedTorusHoleShape(r,s1,s2);
308}
309
310// Given a node, finds a sphere in the plane of n and the centers
311// of _s1 and _s2 that touches the UncappedTorusHole. There are two
312// candidates, the one closest to n is chosen.
313void
314UncappedTorusHoleShape::in_plane_sphere(
315 const SCVector3& n,
316 SCVector3& P) const
317{
318 // the center of the sphere is given by the vector equation
319 // P = A + a R(AB) + b U(perp),
320 // where
321 // A is the center of _s1
322 // B is the center of _s2
323 // R(AB) is the vector from A to B, R(AB) = B - A
324 // U(perp) is a unit vect perp to R(AB) and lies in the plane of n, A, and B
325 // The unknown scalars, a and b are given by solving the following
326 // equations:
327 // | P - A | = r(A) + _r, and
328 // | P - B | = r(B) + _r,
329 // which give
330 // | a R(AB) + b U(perp) | = r(A) + _r, and
331 // | (a-1) R(AB) + b U(perp) | = r(B) + _r.
332 // These further simplify to
333 // a^2 r(AB)^2 + b^2 = (r(A)+_r)^2, and
334 // (a-1)^2 r(AB)^2 + b^2 = (r(B)+_r)^2.
335 // Thus,
336 // a = (((r(A)+_r)^2 - (r(B)+_r)^2 )/(2 r(AB)^2)) + 1/2
337 // b^2 = (r(A)+r)^2 - a^2 r(AB)^2
338
339 SCVector3 A = _s1.origin();
340 SCVector3 B = _s2.origin();
341 SCVector3 N = n;
342 SCVector3 R_AB = B - A;
343 SCVector3 R_AN = N - A;
344
345 // vector projection of R_AN onto R_AB
346 SCVector3 P_AN_AB = R_AB * (R_AN.dot(R_AB)/R_AB.dot(R_AB));
347 // the perpendicular vector
348 SCVector3 U_perp = R_AN - P_AN_AB;
349
350 // if |U| is tiny, then any vector perp to AB will do
351 double u = U_perp.dot(U_perp);
352 if (u<1.0e-23) {
353 SCVector3 vtry = R_AB;
354 vtry[0] += 1.0;
355 vtry = vtry - R_AB * (vtry.dot(R_AB)/R_AB.dot(R_AB));
356 if (vtry.dot(vtry) < 1.0e-23) {
357 vtry = R_AB;
358 vtry[1] += 1.0;
359 vtry = vtry - R_AB * (vtry.dot(R_AB)/R_AB.dot(R_AB));
360 }
361 U_perp = vtry;
362 }
363
364 U_perp.normalize();
365 //ExEnv::outn() << "A: "; A.print();
366 //ExEnv::outn() << "U_perp: "; U_perp.print();
367 //ExEnv::outn() << "R_AB: "; R_AB.print();
368
369 double r_AB = sqrt(R_AB.dot(R_AB));
370 double r_A = _s1.radius();
371 double r_B = _s2.radius();
372
373 double r_Ar = r_A + _r;
374 double r_Br = r_B + _r;
375 double a = ((r_Ar*r_Ar - r_Br*r_Br)/(2.0*r_AB*r_AB)) + 0.5;
376 double b = sqrt(r_Ar*r_Ar - a*a*r_AB*r_AB);
377
378 //ExEnv::outn() << scprintf("r_Ar = %f, r_AB = %f\n",r_Ar,r_AB);
379 //ExEnv::outn() << scprintf("a = %f, b = %f\n",a,b);
380
381 P = A + a * R_AB + b * U_perp;
382 //ExEnv::outn() << "a*R_AB: "; (a*R_AB).print();
383 //ExEnv::outn() << "b*U_perp: "; (b*U_perp).print();
384}
385
386void
387UncappedTorusHoleShape::print(ostream&o) const
388{
389 o << indent << "UncappedTorusHoleShape:" << endl;
390 o << incindent;
391 o << indent << "r = " << _r << endl;
392 o << indent << "s1 = ";
393 o << incindent << skipnextindent;
394 _s1.print(o);
395 o << decindent;
396 o << indent << "s2 = ";
397 o << incindent << skipnextindent;
398 _s2.print(o);
399 o << decindent;
400 o << decindent;
401}
402
403void
404UncappedTorusHoleShape::boundingbox(double valuemin, double valuemax,
405 SCVector3& p1,
406 SCVector3& p2)
407{
408 SCVector3 p11;
409 SCVector3 p12;
410 SCVector3 p21;
411 SCVector3 p22;
412
413 _s1.boundingbox(valuemin,valuemax,p11,p12);
414 _s2.boundingbox(valuemin,valuemax,p21,p22);
415
416 int i;
417 for (i=0; i<3; i++) {
418 if (p11[i] < p21[i]) p1[i] = p11[i];
419 else p1[i] = p21[i];
420 if (p12[i] > p22[i]) p2[i] = p12[i];
421 else p2[i] = p22[i];
422 }
423}
424
425int
426UncappedTorusHoleShape::gradient_implemented() const
427{
428 return 1;
429}
430
431/////////////////////////////////////////////////////////////////////
432// is in triangle
433
434static int
435is_in_unbounded_triangle(const SCVector3&XP,const SCVector3&AP,const SCVector3&BP)
436{
437 SCVector3 axis = BP.cross(AP);
438
439 SCVector3 BP_perp = BP; BP_perp.rotate(M_PI_2,axis);
440 double u = BP_perp.dot(XP)/BP_perp.dot(AP);
441 if (u<0.0) return 0;
442
443 SCVector3 AP_perp = AP; AP_perp.rotate(M_PI_2,axis);
444 double w = AP_perp.dot(XP)/AP_perp.dot(BP);
445 if (w<0.0) return 0;
446
447 return 1;
448}
449
450/////////////////////////////////////////////////////////////////////
451// ReentrantUncappedTorusHoleShape
452
453static ClassDesc ReentrantUncappedTorusHoleShape_cd(
454 typeid(ReentrantUncappedTorusHoleShape),"ReentrantUncappedTorusHoleShape",1,"public UncappedTorusHoleShape",
455 0, 0, 0);
456
457ReentrantUncappedTorusHoleShape::ReentrantUncappedTorusHoleShape(double r,
458 const SphereShape& s1,
459 const SphereShape& s2):
460 UncappedTorusHoleShape(r,s1,s2)
461{
462 rAP = r + s1.radius();
463 rBP = r + s2.radius();
464 BA = B() - A();
465
466 // Find the points at the ends of the two cone-like objects, I[0] and I[1].
467 // they are given by:
468 // I = z BA, where BA = B-A and I is actually IA = I - A
469 // r^2 = PI.PI, where PI = PA-I and P is the center of a probe sphere
470 // this gives
471 // z^2 BA.BA - 2z PA.BA + PA.PA - r^2 = 0
472
473 SCVector3 arbitrary;
474 arbitrary[0] = arbitrary[1] = arbitrary[2] = 0.0;
475 SCVector3 P;
476 in_plane_sphere(arbitrary,P);
477 SCVector3 PA = P - A();
478
479 double a = BA.dot(BA);
480 double minus_b = 2.0 * PA.dot(BA);
481 double c = PA.dot(PA) - r*r;
482 double b2m4ac = minus_b*minus_b - 4*a*c;
483 double sb2m4ac;
484 if (b2m4ac >= 0.0) {
485 sb2m4ac = sqrt(b2m4ac);
486 }
487 else if (b2m4ac > -1.0e-10) {
488 sb2m4ac = 0.0;
489 }
490 else {
491 ExEnv::errn() << "ReentrantUncappedTorusHoleShape:: imaginary point" << endl;
492 abort();
493 }
494 double zA = (minus_b - sb2m4ac)/(2.0*a);
495 double zB = (minus_b + sb2m4ac)/(2.0*a);
496 I[0] = BA * zA + A();
497 I[1] = BA * zB + A();
498}
499ReentrantUncappedTorusHoleShape::~ReentrantUncappedTorusHoleShape()
500{
501}
502int
503ReentrantUncappedTorusHoleShape::
504 is_outside(const SCVector3&X) const
505{
506 SCVector3 Xv(X);
507
508 SCVector3 P;
509 in_plane_sphere(X,P);
510 SCVector3 XP = Xv - P;
511 double rXP = XP.norm();
512 if (rXP > rAP || rXP > rBP) return 1;
513
514 SCVector3 AP = A() - P;
515 SCVector3 BP = B() - P;
516
517 if (!is_in_unbounded_triangle(XP,AP,BP)) return 1;
518
519 if (rXP < radius()) {
520 return 1;
521 }
522
523 return 0;
524}
525double
526ReentrantUncappedTorusHoleShape::
527 distance_to_surface(const SCVector3&X,SCVector3*grad) const
528{
529 SCVector3 Xv(X);
530
531 SCVector3 P;
532 in_plane_sphere(X,P);
533 SCVector3 XP = Xv - P;
534 double rXP = XP.norm();
535 if (rXP > rAP || rXP > rBP) return shape_infinity;
536
537 SCVector3 AP = A() - P;
538 SCVector3 BP = B() - P;
539
540 if (!is_in_unbounded_triangle(XP,AP,BP)) return shape_infinity;
541
542 SCVector3 I1P = I[0] - P;
543 SCVector3 I2P = I[1] - P;
544
545 if (!is_in_unbounded_triangle(XP,I1P,I2P)) {
546 if (rXP < radius()) {
547 if (grad) {
548 SCVector3 unit(XP);
549 unit.normalize();
550 *grad = unit;
551 }
552 return radius() - rXP;
553 }
554 else return -1.0;
555 }
556
557 return closest_distance(Xv,(SCVector3*)I,2,grad);
558}
559
560int
561ReentrantUncappedTorusHoleShape::gradient_implemented() const
562{
563 return 1;
564}
565
566/////////////////////////////////////////////////////////////////////
567// NonreentrantUncappedTorusHoleShape
568
569static ClassDesc NonreentrantUncappedTorusHoleShape_cd(
570 typeid(NonreentrantUncappedTorusHoleShape),"NonreentrantUncappedTorusHoleShape",1,"public UncappedTorusHoleShape",
571 0, 0, 0);
572
573NonreentrantUncappedTorusHoleShape::
574 NonreentrantUncappedTorusHoleShape(double r,
575 const SphereShape& s1,
576 const SphereShape& s2):
577 UncappedTorusHoleShape(r,s1,s2)
578{
579 rAP = r + s1.radius();
580 rBP = r + s2.radius();
581 BA = B() - A();
582}
583NonreentrantUncappedTorusHoleShape::~NonreentrantUncappedTorusHoleShape()
584{
585}
586double NonreentrantUncappedTorusHoleShape::
587 distance_to_surface(const SCVector3&X,SCVector3* grad) const
588{
589 SCVector3 Xv(X);
590
591 SCVector3 P;
592 in_plane_sphere(X,P);
593 SCVector3 PX = P - Xv;
594 double rPX = PX.norm();
595 if (rPX > rAP || rPX > rBP) return shape_infinity;
596
597 SCVector3 PA = P - A();
598 SCVector3 XA = Xv - A();
599
600 SCVector3 axis = BA.cross(PA);
601
602 SCVector3 BA_perp = BA; BA_perp.rotate(M_PI_2,axis);
603 double u = BA_perp.dot(XA)/BA_perp.dot(PA);
604 if (u<0.0 || u>1.0) return shape_infinity;
605
606 SCVector3 PA_perp = PA; PA_perp.rotate(M_PI_2,axis);
607 double w = PA_perp.dot(XA)/PA_perp.dot(BA);
608 if (w<0.0 || w>1.0) return shape_infinity;
609
610 double uw = u+w;
611 if (uw<0.0 || uw>1.0) return shape_infinity;
612
613 if (rPX < radius()) {
614 if (grad) {
615 SCVector3 unit(PX);
616 unit.normalize();
617 *grad = unit;
618 }
619 return radius() - rPX;
620 }
621
622 return -1;
623}
624
625int
626NonreentrantUncappedTorusHoleShape::gradient_implemented() const
627{
628 return 1;
629}
630
631/////////////////////////////////////////////////////////////////////
632// Uncapped5SphereExclusionShape
633
634static ClassDesc Uncapped5SphereExclusionShape_cd(
635 typeid(Uncapped5SphereExclusionShape),"Uncapped5SphereExclusionShape",1,"public Shape",
636 0, 0, 0);
637
638Uncapped5SphereExclusionShape*
639Uncapped5SphereExclusionShape::
640 newUncapped5SphereExclusionShape(double r,
641 const SphereShape& s1,
642 const SphereShape& s2,
643 const SphereShape& s3)
644{
645 Uncapped5SphereExclusionShape* s =
646 new Uncapped5SphereExclusionShape(r,s1,s2,s3);
647 if (s->solution_exists()) {
648 return s;
649 }
650 delete s;
651 return 0;
652}
653static int verbose = 0;
654Uncapped5SphereExclusionShape::
655 Uncapped5SphereExclusionShape(double radius,
656 const SphereShape&s1,
657 const SphereShape&s2,
658 const SphereShape&s3):
659 _s1(s1),
660 _s2(s2),
661 _s3(s3),
662 _r(radius)
663{
664 double rAr = rA() + r();
665 double rAr2 = rAr*rAr;
666 double rBr = rB() + r();
667 double rBr2 = rBr*rBr;
668 double rCr = rC() + r();
669 double rCr2 = rCr*rCr;
670 double A2 = A().dot(A());
671 double B2 = B().dot(B());
672 double C2 = C().dot(C());
673 SCVector3 BA = B()-A();
674 double DdotBA = 0.5*(rAr2 - rBr2 + B2 - A2);
675 double DAdotBA = DdotBA - A().dot(BA);
676 double BA2 = BA.dot(BA);
677 SCVector3 CA = C() - A();
678 double CAdotBA = CA.dot(BA);
679 SCVector3 CA_perpBA = CA - (CAdotBA/BA2)*BA;
680 double CA_perpBA2 = CA_perpBA.dot(CA_perpBA);
681 if (CA_perpBA2 < 1.0e-23) {
682 _solution_exists = 0;
683 return;
684 }
685 double DdotCA_perpBA = 0.5*(rAr2 - rCr2 + C2 - A2)
686 - CAdotBA*DdotBA/BA2;
687 double DAdotCA_perpBA = DdotCA_perpBA - A().dot(CA_perpBA);
688 double rAt2 = DAdotBA*DAdotBA/BA2 + DAdotCA_perpBA*DAdotCA_perpBA/CA_perpBA2;
689 double h2 = rAr2 - rAt2;
690 if (h2 <= 0.0) {
691 _solution_exists = 0;
692 return;
693 }
694 _solution_exists = 1;
695
696 double h = sqrt(h2);
697 if (h<r()) {
698 _reentrant = 1;
699 //ExEnv::outn() << "WARNING: throwing out reentrant shape" << endl;
700 //_solution_exists = 0;
701 //return;
702 }
703 else {
704 _reentrant = 0;
705 //ExEnv::outn() << "WARNING: throwing out nonreentrant shape" << endl;
706 //_solution_exists = 0;
707 //return;
708 }
709
710 // The projection of D into the ABC plane
711 SCVector3 MA = (DAdotBA/BA2)*BA + (DAdotCA_perpBA/CA_perpBA2)*CA_perpBA;
712 M = MA + A();
713 SCVector3 BAxCA = BA.cross(CA);
714 D[0] = M + h * BAxCA * ( 1.0/BAxCA.norm() );
715 D[1] = M - h * BAxCA * ( 1.0/BAxCA.norm() );
716
717 // The projection of D into the ABC plane, M, does not lie in the
718 // ABC triangle, then this shape must be treated carefully and it
719 // will be designated as folded.
720 SCVector3 MC = M - C();
721 if (!(is_in_unbounded_triangle(MA, BA, CA)
722 &&is_in_unbounded_triangle(MC, B() - C(), A() - C()))) {
723 _folded = 1;
724 SCVector3 MB = M - B();
725 double MA2 = MA.dot(MA);
726 double MB2 = MB.dot(MB);
727 double MC2 = MC.dot(MC);
728 if (MA2 < MB2) {
729 F1 = A();
730 if (MB2 < MC2) F2 = B();
731 else F2 = C();
732 }
733 else {
734 F1 = B();
735 if (MA2 < MC2) F2 = A();
736 else F2 = C();
737 }
738 }
739 else _folded = 0;
740
741 //ExEnv::outn() << scprintf("r = %14.8f, h = %14.8f\n",r(),h);
742 //M.print();
743 //D[0].print();
744 //D[1].print();
745 //A().print();
746 //B().print();
747 //C().print();
748
749 int i;
750 for (i=0; i<2; i++) {
751 SCVector3 AD = A() - D[i];
752 SCVector3 BD = B() - D[i];
753 SCVector3 CD = C() - D[i];
754 BDxCD[i] = BD.cross(CD);
755 BDxCDdotAD[i] = BDxCD[i].dot(AD);
756 CDxAD[i] = CD.cross(AD);
757 CDxADdotBD[i] = CDxAD[i].dot(BD);
758 ADxBD[i] = AD.cross(BD);
759 ADxBDdotCD[i] = ADxBD[i].dot(CD);
760 }
761
762 for (i=0; i<2; i++) MD[i] = M - D[i];
763
764 // reentrant surfaces need a whole bunch more to be able to compute
765 // the distance to the surface
766 if (_reentrant) {
767 int i;
768 double rMD = MD[0].norm(); // this is the same as rMD[1]
769 theta_intersect = M_PI_2 - asin(rMD/r());
770 r_intersect = r() * sin(theta_intersect);
771
772 {
773 // Does the circle of intersection intersect with BA?
774 SCVector3 MA = M - A();
775 SCVector3 uBA = B() - A(); uBA.normalize();
776 SCVector3 XA = uBA * MA.dot(uBA);
777 SCVector3 XM = XA - MA;
778 double rXM2 = XM.dot(XM);
779 double d_intersect_from_x2 = r_intersect*r_intersect - rXM2;
780 if (d_intersect_from_x2 > 0.0) {
781 _intersects_AB = 1;
782 double tmp = sqrt(d_intersect_from_x2);
783 double d_intersect_from_x[2];
784 d_intersect_from_x[0] = tmp;
785 d_intersect_from_x[1] = -tmp;
786 for (i=0; i<2; i++) {
787 for (int j=0; j<2; j++) {
788 IABD[i][j] = XM + d_intersect_from_x[j]*uBA + MD[i];
789 }
790 }
791 }
792 else _intersects_AB = 0;
793 }
794
795 {
796 // Does the circle of intersection intersect with BC?
797 SCVector3 MC = M - C();
798 SCVector3 uBC = B() - C(); uBC.normalize();
799 SCVector3 XC = uBC * MC.dot(uBC);
800 SCVector3 XM = XC - MC;
801 double rXM2 = XM.dot(XM);
802 double d_intersect_from_x2 = r_intersect*r_intersect - rXM2;
803 if (d_intersect_from_x2 > 0.0) {
804 _intersects_BC = 1;
805 double tmp = sqrt(d_intersect_from_x2);
806 double d_intersect_from_x[2];
807 d_intersect_from_x[0] = tmp;
808 d_intersect_from_x[1] = -tmp;
809 for (i=0; i<2; i++) {
810 for (int j=0; j<2; j++) {
811 IBCD[i][j] = XM + d_intersect_from_x[j]*uBC + MD[i];
812 }
813 }
814 }
815 else _intersects_BC = 0;
816 }
817
818 {
819 // Does the circle of intersection intersect with CA?
820 SCVector3 MA = M - A();
821 SCVector3 uCA = C() - A(); uCA.normalize();
822 SCVector3 XA = uCA * MA.dot(uCA);
823 SCVector3 XM = XA - MA;
824 double rXM2 = XM.dot(XM);
825 double d_intersect_from_x2 = r_intersect*r_intersect - rXM2;
826 if (d_intersect_from_x2 > 0.0) {
827 _intersects_CA = 1;
828 double tmp = sqrt(d_intersect_from_x2);
829 double d_intersect_from_x[2];
830 d_intersect_from_x[0] = tmp;
831 d_intersect_from_x[1] = -tmp;
832 for (i=0; i<2; i++) {
833 for (int j=0; j<2; j++) {
834 ICAD[i][j] = XM + d_intersect_from_x[j]*uCA + MD[i];
835 }
836 }
837 }
838 else _intersects_CA = 0;
839 }
840
841 }
842
843#if 0 // test code
844 ExEnv::outn() << "Uncapped5SphereExclusionShape: running some tests" << endl;
845 verbose = 1;
846
847 FILE* testout = fopen("testout.vect", "w");
848
849 const double scalefactor_inc = 0.05;
850 const double start = -10.0;
851 const double end = 10.0;
852
853 SCVector3 middle = (1.0/3.0)*(A()+B()+C());
854
855 int nlines = 1;
856 int nvert = (int) ( (end-start) / scalefactor_inc);
857 int ncolor = nvert;
858
859 fprintf(testout, "VECT\n%d %d %d\n", nlines, nvert, ncolor);
860
861 fprintf(testout, "%d\n", nvert);
862 fprintf(testout, "%d\n", ncolor);
863
864 double scalefactor = start;
865 for (int ii = 0; ii<nvert; ii++) {
866 SCVector3 position = (D[0] - middle) * scalefactor + middle;
867 double d = distance_to_surface(position);
868 fprintf(testout, "%f %f %f # value = %f\n",
869 position[0], position[1], position[2], d);
870 scalefactor += scalefactor_inc;
871 }
872 scalefactor = start;
873 for (ii = 0; ii<nvert; ii++) {
874 SCVector3 position = (D[0] - middle) * scalefactor + middle;
875 double d = distance_to_surface(position);
876 ExEnv::outn() << scprintf("d = %f\n", d);
877 if (d<0.0) fprintf(testout,"1.0 0.0 0.0 0.5\n");
878 else fprintf(testout,"0.0 0.0 1.0 0.5\n");
879 scalefactor += scalefactor_inc;
880 }
881
882 fclose(testout);
883 ExEnv::outn() << "testout.vect written" << endl;
884
885 verbose = 0;
886#endif // test code
887
888}
889int
890Uncapped5SphereExclusionShape::is_outside(const SCVector3&X) const
891{
892 SCVector3 Xv(X);
893
894 if (verbose) ExEnv::outn() << scprintf("point %14.8f %14.8f %14.8f\n",X(0),X(1),X(2));
895
896 // The folded case isn't handled correctly here, so use
897 // the less efficient distance_to_surface routine.
898 if (_folded) {
899 return distance_to_surface(X) >= 0.0;
900 }
901
902 for (int i=0; i<2; i++) {
903 SCVector3 XD = Xv - D[i];
904 double rXD = XD.norm();
905 if (rXD <= r()) return 1;
906 double u = BDxCD[i].dot(XD)/BDxCDdotAD[i];
907 if (u <= 0.0) return 1;
908 double v = CDxAD[i].dot(XD)/CDxADdotBD[i];
909 if (v <= 0.0) return 1;
910 double w = ADxBD[i].dot(XD)/ADxBDdotCD[i];
911 if (w <= 0.0) return 1;
912 }
913
914 if (verbose) ExEnv::outn() << "is_inside" << endl;
915
916 return 0;
917}
918static int
919is_contained_in_unbounded_pyramid(SCVector3 XD,
920 SCVector3 AD,
921 SCVector3 BD,
922 SCVector3 CD)
923{
924 SCVector3 BDxCD = BD.cross(CD);
925 SCVector3 CDxAD = CD.cross(AD);
926 SCVector3 ADxBD = AD.cross(BD);
927 double u = BDxCD.dot(XD)/BDxCD.dot(AD);
928 if (u <= 0.0) return 0;
929 double v = CDxAD.dot(XD)/CDxAD.dot(BD);
930 if (v <= 0.0) return 0;
931 double w = ADxBD.dot(XD)/ADxBD.dot(CD);
932 if (w <= 0.0) return 0;
933 return 1;
934}
935double
936Uncapped5SphereExclusionShape::
937 distance_to_surface(const SCVector3&X,SCVector3*grad) const
938{
939 SCVector3 Xv(X);
940
941 // Find out if I'm on the D[0] side or the D[1] side of the ABC plane.
942 int side;
943 SCVector3 XM = Xv - M;
944 if (MD[0].dot(XM) > 0.0) side = 1;
945 else side = 0;
946
947 if (verbose) {
948 ExEnv::outn() << scprintf("distance_to_surface: folded = %d, side = %d\n",
949 _folded, side);
950 ExEnv::outn() << "XM = "; XM.print();
951 ExEnv::outn() << "MD[0] = "; MD[0].print();
952 ExEnv::outn() << "MD[0].dot(XM) = " << MD[0].dot(XM) << endl;
953 }
954
955 SCVector3 XD = Xv - D[side];
956 double u = BDxCD[side].dot(XD)/BDxCDdotAD[side];
957 if (verbose) ExEnv::outn() << scprintf("u = %14.8f\n", u);
958 if (u <= 0.0) return shape_infinity;
959 double v = CDxAD[side].dot(XD)/CDxADdotBD[side];
960 if (verbose) ExEnv::outn() << scprintf("v = %14.8f\n", v);
961 if (v <= 0.0) return shape_infinity;
962 double w = ADxBD[side].dot(XD)/ADxBDdotCD[side];
963 if (verbose) ExEnv::outn() << scprintf("w = %14.8f\n", w);
964 if (w <= 0.0) return shape_infinity;
965 double rXD = XD.norm();
966 if (verbose) ExEnv::outn() << scprintf("r() - rXD = %14.8f\n", r() - rXD);
967 if (rXD <= r()) {
968 if (!_reentrant) return r() - rXD;
969 // this shape is reentrant
970 // make sure that we're on the right side
971 if ((side == 1) || (u + v + w <= 1.0)) {
972 // see if we're outside the cone that intersects
973 // the opposite sphere
974 double angle = acos(fabs(XD.dot(MD[side]))
975 /(XD.norm()*MD[side].norm()));
976 if (angle >= theta_intersect) {
977 if (grad) {
978 *grad = (-1.0/rXD)*XD;
979 }
980 return r() - rXD;
981 }
982 if (_intersects_AB
983 &&is_contained_in_unbounded_pyramid(XD,
984 MD[side],
985 IABD[side][0],
986 IABD[side][1])) {
987 //ExEnv::outn() << scprintf("XD: "); XD.print();
988 //ExEnv::outn() << scprintf("MD[%d]: ",i); MD[i].print();
989 //ExEnv::outn() << scprintf("IABD[%d][0]: ",i); IABD[i][0].print();
990 //ExEnv::outn() << scprintf("IABD[%d][1]: ",i); IABD[i][1].print();
991 return closest_distance(XD,(SCVector3*)IABD[side],2,grad);
992 }
993 if (_intersects_BC
994 &&is_contained_in_unbounded_pyramid(XD,
995 MD[side],
996 IBCD[side][0],
997 IBCD[side][1])) {
998 return closest_distance(XD,(SCVector3*)IBCD[side],2,grad);
999 }
1000 if (_intersects_CA
1001 &&is_contained_in_unbounded_pyramid(XD,
1002 MD[side],
1003 ICAD[side][0],
1004 ICAD[side][1])) {
1005 return closest_distance(XD,(SCVector3*)ICAD[side],2,grad);
1006 }
1007 // at this point we are closest to the ring formed
1008 // by the intersection of the two probe spheres
1009 double distance_to_plane;
1010 double distance_to_ring_in_plane;
1011 double MDnorm = MD[side].norm();
1012 SCVector3 XM = XD - MD[side];
1013 SCVector3 XM_in_plane;
1014 if (MDnorm<1.0e-16) {
1015 distance_to_plane = 0.0;
1016 XM_in_plane = XD;
1017 }
1018 else {
1019 distance_to_plane = XM.dot(MD[side])/MDnorm;
1020 XM_in_plane = XM - (distance_to_plane/MDnorm)*MD[side];
1021 }
1022 if (grad) {
1023 double XM_in_plane_norm = XM_in_plane.norm();
1024 if (XM_in_plane_norm < 1.e-8) {
1025 // the grad points along MD
1026 double scale = - distance_to_plane
1027 /(MDnorm*sqrt(r_intersect*r_intersect
1028 + distance_to_plane*distance_to_plane));
1029 *grad = MD[side] * scale;
1030 }
1031 else {
1032 SCVector3 point_on_ring;
1033 point_on_ring = XM_in_plane
1034 * (r_intersect/XM_in_plane_norm) + M;
1035 SCVector3 gradv = Xv - point_on_ring;
1036 gradv.normalize();
1037 *grad = gradv;
1038 }
1039 }
1040 distance_to_ring_in_plane =
1041 r_intersect - sqrt(XM_in_plane.dot(XM_in_plane));
1042 return sqrt(distance_to_ring_in_plane*distance_to_ring_in_plane
1043 +distance_to_plane*distance_to_plane);
1044 }
1045 }
1046
1047 if (verbose) ExEnv::outn() << "returning -1.0" << endl;
1048 return -1.0;
1049}
1050
1051void
1052Uncapped5SphereExclusionShape::boundingbox(double valuemin, double valuemax,
1053 SCVector3& p1,
1054 SCVector3& p2)
1055{
1056 SCVector3 p11;
1057 SCVector3 p12;
1058 SCVector3 p21;
1059 SCVector3 p22;
1060 SCVector3 p31;
1061 SCVector3 p32;
1062
1063 _s1.boundingbox(valuemin,valuemax,p11,p12);
1064 _s2.boundingbox(valuemin,valuemax,p21,p22);
1065 _s3.boundingbox(valuemin,valuemax,p31,p32);
1066
1067 int i;
1068 for (i=0; i<3; i++) {
1069 if (p11[i] < p21[i]) p1[i] = p11[i];
1070 else p1[i] = p21[i];
1071 if (p31[i] < p1[i]) p1[i] = p31[i];
1072 if (p12[i] > p22[i]) p2[i] = p12[i];
1073 else p2[i] = p22[i];
1074 if (p32[i] > p2[i]) p2[i] = p32[i];
1075 }
1076}
1077
1078int
1079Uncapped5SphereExclusionShape::gradient_implemented() const
1080{
1081 return 1;
1082}
1083
1084/////////////////////////////////////////////////////////////////////
1085// Unionshape
1086
1087static ClassDesc UnionShape_cd(
1088 typeid(UnionShape),"UnionShape",1,"public Shape",
1089 0, 0, 0);
1090
1091UnionShape::UnionShape()
1092{
1093}
1094
1095UnionShape::~UnionShape()
1096{
1097}
1098
1099void
1100UnionShape::add_shape(Ref<Shape> s)
1101{
1102 _shapes.insert(s);
1103}
1104
1105// NOTE: this underestimates the distance to the surface when
1106//inside the surface
1107double
1108UnionShape::distance_to_surface(const SCVector3&p,SCVector3* grad) const
1109{
1110 std::set<Ref<Shape> >::const_iterator imin = _shapes.begin();
1111 if (imin == _shapes.end()) return 0.0;
1112 double min = (*imin)->distance_to_surface(p);
1113 for (std::set<Ref<Shape> >::const_iterator i=imin; i!=_shapes.end(); i++) {
1114 double d = (*i)->distance_to_surface(p);
1115 if (min <= 0.0) {
1116 if (d < 0.0 && d > min) { min = d; imin = i; }
1117 }
1118 else {
1119 if (min > d) { min = d; imin = i; }
1120 }
1121 }
1122
1123 if (grad) {
1124 (*imin)->distance_to_surface(p,grad);
1125 }
1126 return min;
1127}
1128
1129int
1130UnionShape::is_outside(const SCVector3&p) const
1131{
1132 for (std::set<Ref<Shape> >::const_iterator i=_shapes.begin();
1133 i!=_shapes.end(); i++) {
1134 if (!(*i)->is_outside(p)) return 0;
1135 }
1136
1137 return 1;
1138}
1139
1140void
1141UnionShape::boundingbox(double valuemin, double valuemax,
1142 SCVector3& p1,
1143 SCVector3& p2)
1144{
1145 if (_shapes.begin() == _shapes.end()) {
1146 for (int i=0; i<3; i++) p1[i] = p2[i] = 0.0;
1147 return;
1148 }
1149
1150 SCVector3 pt1;
1151 SCVector3 pt2;
1152
1153 std::set<Ref<Shape> >::iterator j = _shapes.begin();
1154 int i;
1155 (*j)->boundingbox(valuemin,valuemax,p1,p2);
1156 for (j++; j!=_shapes.end(); j++) {
1157 (*j)->boundingbox(valuemin,valuemax,pt1,pt2);
1158 for (i=0; i<3; i++) {
1159 if (pt1[i] < p1[i]) p1[i] = pt1[i];
1160 if (pt2[i] > p2[i]) p2[i] = pt2[i];
1161 }
1162 }
1163}
1164
1165int
1166UnionShape::gradient_implemented() const
1167{
1168 for (std::set<Ref<Shape> >::const_iterator j=_shapes.begin();
1169 j!=_shapes.end(); j++) {
1170 if (!(*j)->gradient_implemented()) return 0;
1171 }
1172 return 1;
1173}
1174
1175/////////////////////////////////////////////////////////////////////////////
1176
1177// Local Variables:
1178// mode: c++
1179// c-file-style: "CLJ"
1180// End:
Note: See TracBrowser for help on using the repository browser.