www.pudn.com > MyGame.rar > Sphere3.h
/*****************************************************************************
* VCGLib *
* *
* Visual Computing Group o> *
* IEI Institute, CNUCE Institute, CNR Pisa <| *
* / \ *
* Copyright(C) 1999 by Paolo Cignoni, Claudio Rocchini *
* All rights reserved. *
* *
* Permission to use, copy, modify, distribute and sell this software and *
* its documentation for any purpose is hereby granted without fee, provided *
* that the above copyright notice appear in all copies and that both that *
* copyright notice and this permission notice appear in supporting *
* documentation. the author makes no representations about the suitability *
* of this software for any purpose. It is provided "as is" without express *
* or implied warranty. *
* *
*****************************************************************************/
/****************************************************************************
History
1999 Nov 02 First Draft.
*/
#ifndef __VCGLIB_SPHERE3
#define __VCGLIB_SPHERE3
#include
#include
#include
namespace vcg {
template class Sphere3
{
public:
Point3 c;
FLTYPE r;
inline Sphere3( void ){}
inline Sphere3(Point3 const & cc,FLTYPE const & rr){c=cc;r=rr;}
inline Sphere3(Point3 const & cc){c=cc;r=0;}
inline int Circumscribe(Point3 &v0,Point3 &v1,Point3 &v2,Point3 &v3);
bool operator < (Sphere3 const & s) const {
if(c!=s.c) return c const & p) const {
if((c-p).Norm2() < r*r) return true;
else return false;
}
inline bool Intersect(Sphere3 const & s) const {
if((c-s.c).Norm()< r+s.r) return true;
else return false;
}
inline FLTYPE Distance( Sphere3 const & s ) const{
return (c-s.c).Norm()-(r+s.r);
}
// It is not faster that Distance!!!
inline FLTYPE SquaredDistance( Sphere3 const & s ) const{
return (c-s.c).Norm()-(r+s.r)*(c-s.c).Norm()-(r+s.r);
}
/* Sphere3::Inscribe
Si assume che i punti describano i vertici di un tetraedro ben orientato
cioe v0 vede v1v2v3 in senso antiorario;
La sfera inscritta a un tetraedro verifica che:
Ni*(c-vi) = r
cioe' il centro e' equidistante dai piani dele facce del tetraedro
(Ni sono le normali alle facce orientate verso L'INTERNO!!);
Si puo' riscrivere come:
(Ni.x,Ni.y,Ni.z,-1)*(c.x,c.y,c.z,r) = Ni*vi
Che si risolve facilmente con un sistema 4x4;
Osservando la matrice e sottraendo (N0,-1) a tutti le righe del sistema
viene la prima riga e l'ultima colonna di zeri e si puo' risolvere
piu' rapidamente con l'inversa di una 3x3
*/
int Inscribe(Point3 const &v0,Point3 const &v1,Point3 const &v2,Point3 const &v3)
{
// Egdes (nota basterebbe calcolarne solo 4...)
Point3 e10=v1-v0;
Point3 e20=v2-v0;
Point3 e30=v3-v0;
Point3 e21=v2-v1;
Point3 e31=v3-v1;
// Face Normals
Point3 n0 = e21 ^ e31;
Point3 n1 = e30 ^ e20;
Point3 n2 = e10 ^ e30;
Point3 n3 = e20 ^ e10;
// Si dovrebbe controllare che le normali non sono degeneri
n0.Normalize();
n1.Normalize();
n2.Normalize();
n3.Normalize();
FLTYPE A[3][3];
A[0][0] = n1.v[0]-n0.v[0]; A[0][1] = n1.v[1]-n0.v[1]; A[0][2] = n1.v[2]-n0.v[2];
A[1][0] = n2.v[0]-n0.v[0]; A[1][1] = n2.v[1]-n0.v[1]; A[1][2] = n2.v[2]-n0.v[2];
A[2][0] = n3.v[0]-n0.v[0]; A[2][1] = n3.v[1]-n0.v[1]; A[2][2] = n3.v[2]-n0.v[2];
FLTYPE b[3];
b[0] = 0;
b[1] = 0;
b[2] = -n3.v[0]*e30.v[0]-n3.v[1]*e30.v[1]-n3.v[2]*e30.v[2];
FLTYPE sol[3];
if ( !Solve3x3(A,b,sol) ){
r=0;
//printf("Sphere3::Inscribe: Unable to solve the system\n") ;
return 0;
}
c.v[0] = v3.v[0]+sol[0];
c.v[1] = v3.v[1]+sol[1];
c.v[2] = v3.v[2]+sol[2];
r = n0.v[0]*sol[0]+n0.v[1]*sol[1]+n0.v[2]*sol[2];
if(r<0) r = -r;
return 1;
}
private:
//-- Nota che questa funzione deve sparire!!!
int Solve2x2 (FLTYPE A[2][2], FLTYPE b[2], FLTYPE x[2])
{
FLTYPE det = A[0][0]*A[1][1]-A[0][1]*A[1][0];
if ( fabs(det) < 1e-06 )
return 0;
FLTYPE invDet = 1.0/det;
x[0] = (A[1][1]*b[0]-A[0][1]*b[1])*invDet;
x[1] = (A[0][0]*b[1]-A[1][0]*b[0])*invDet;
return 1;
}
//-- Nota che questa funzione deve sparire!!!
int Solve3x3 (FLTYPE A[3][3], FLTYPE b[3], FLTYPE x[3])
{
FLTYPE Ainv[3][3];
Ainv[0][0] = A[1][1]*A[2][2]-A[1][2]*A[2][1];
Ainv[0][1] = A[0][2]*A[2][1]-A[0][1]*A[2][2];
Ainv[0][2] = A[0][1]*A[1][2]-A[0][2]*A[1][1];
Ainv[1][0] = A[1][2]*A[2][0]-A[1][0]*A[2][2];
Ainv[1][1] = A[0][0]*A[2][2]-A[0][2]*A[2][0];
Ainv[1][2] = A[0][2]*A[1][0]-A[0][0]*A[1][2];
Ainv[2][0] = A[1][0]*A[2][1]-A[1][1]*A[2][0];
Ainv[2][1] = A[0][1]*A[2][0]-A[0][0]*A[2][1];
Ainv[2][2] = A[0][0]*A[1][1]-A[0][1]*A[1][0];
FLTYPE dDet = A[0][0]*Ainv[0][0]+A[0][1]*Ainv[1][0]+A[0][2]*Ainv[2][0];
if ( fabs(dDet) < 1e-06 )
return 0;
FLTYPE dInvdet = 1.0/dDet;
int row, col;
for (row = 0; row < 3; row++)
{
for (col = 0; col < 3; col++)
{
Ainv[row][col] *= dInvdet;
}
}
for (row = 0; row < 3; row++)
{
x[row] = 0.0;
for (col = 0; col < 3; col++)
{
x[row] += Ainv[row][col]*b[col];
}
}
return 1;
}
}; // End Class
/** Algorithms Working with spheres and points */
template
inline FLTYPE Distance( Sphere3 const & s,Point3 const & p ){
return Norm(s.c-p)-s.r;
}
template
inline FLTYPE SquaredDistance( Sphere3 const & s,Point3 const & p ){
return SquaredNorm(s.c-p)-s.r*s.r;
}
template
inline FLTYPE Distance( Sphere3 const & s1,Sphere3 const & s2 ){
return Norm(s1.c-s2.c)-(s1.r+s2.r);
}
template
inline FLTYPE SquaredDistance( Sphere3 const & s,Sphere3 const & p ){
return Norm(s1.c-s2.c)-(s1.r+s2.r)*Norm(s1.c-s2.c)-(s1.r+s2.r);
}
/** Algorithms Returning Spheres */
template
inline Sphere3 MinSphere2 (Point3 const &p0, Point3 const &p1)
{
Sphere3 minimal;
minimal.c.v[0] = 0.5*(p0.v[0]+p1.v[0]);
minimal.c.v[1] = 0.5*(p0.v[1]+p1.v[1]);
minimal.c.v[2] = 0.5*(p0.v[2]+p1.v[2]);
double dx = p1.v[0]-p0.v[0];
double dy = p1.v[1]-p0.v[1];
double dz = p1.v[2]-p0.v[2];
minimal.r = 0.5*vcg::sqrt(dx*dx+dy*dy+dz*dz);
return minimal;
}
template
inline Sphere3 MinSphere2 (Point3 const &p0, Point3 const &p1, Point3 const &p2)
{
// Compute the circle (in 3D) containing p0, p1, and p2. The center in
// barycentric coordinates is C = u0*P0+u1*P1+u2*P2 where u0+u1+u2=1,
// 0 < u0 < 1, 0 < u1 < 1, and 0 < u2 < 1. The center is equidistant
// from the three points, so |C-p0| = |C-p1| = |C-p2| = R where R is the
// radius of the circle.
//
// From these conditions,
// C-p0 = u0*A + u1*B - A
// C-p1 = u0*A + u1*B - B
// C-p2 = u0*A + u1*B
// where A = P0-P2 and B = P1-P2, which leads to
// r^2 = |u0*A+u1*B|^2 - 2*Dot(A,u0*A+u1*B) + |A|^2
// r^2 = |u0*A+u1*B|^2 - 2*Dot(B,u0*A+u1*B) + |B|^2
// r^2 = |u0*A+u1*B|^2
// Subtracting the last equation from the first two and writing
// the equations as a linear system,
//
// +- -++ -+ +- -+
// | Dot(A,A) Dot(A,B) || u0 | = 0.5 | Dot(A,A) |
// | Dot(A,B) Dot(B,B) || u1 | | Dot(B,B) |
// +- -++ -+ +- -+
//
// The following code solves this system for u0 and u1, then
// evaluates the third equation in r^2 to obtain r.
Sphere3 minimal;
Point3 A( p0.v[0]-p2.v[0], p0.v[1]-p2.v[1], p0.v[2]-p2.v[2]);
Point3 B(p1.v[0]-p2.v[0], p1.v[1]-p2.v[1], p1.v[2]-p2.v[2]);
double AA = A.v[0]*A.v[0]+A.v[1]*A.v[1]+A.v[2]*A.v[2];
FLTYPE AB = A.v[0]*B.v[0]+A.v[1]*B.v[1]+A.v[2]*B.v[2];
FLTYPE BB = B.v[0]*B.v[0]+B.v[1]*B.v[1]+B.v[2]*B.v[2];
FLTYPE det = AA*BB-AB*AB;
if ( fabs(det) > 1e-06 )
{
FLTYPE halfInvDet = 0.5f/(AA*BB-AB*AB);
FLTYPE u0 = halfInvDet*BB*(AA-AB);
FLTYPE u1 = halfInvDet*AA*(BB-AB);
FLTYPE u2 = 1.0f-u0-u1;
Point3 tmp(u0*A.v[0]+u1*B.v[0], u0*A.v[1]+u1*B.v[1], u0*A.v[2]+u1*B.v[2] );
minimal.c.v[0] = u0*p0.v[0]+u1*p1.v[0]+u2*p2.v[0];
minimal.c.v[1] = u0*p0.v[1]+u1*p1.v[1]+u2*p2.v[1];
minimal.c.v[2] = u0*p0.v[2]+u1*p1.v[2]+u2*p2.v[2];
minimal.r = sqrt(tmp.v[0]*tmp.v[0]+tmp.v[1]*tmp.v[1]+tmp.v[2]*tmp.v[2]);
}
else
{
minimal.c.v[0] = DBL_MAX;
minimal.c.v[1] = DBL_MAX;
minimal.c.v[2] = DBL_MAX;
minimal.r = DBL_MAX;
}
return minimal;
}
typedef Sphere3 Sphere3s;
typedef Sphere3 Sphere3i;
typedef Sphere3 Sphere3f;
typedef Sphere3 Sphere3d;
} // end namespace
#endif