sphere3.h

三维空战VC源码

/*****************************************************************************
 * 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 <assert.h>
#include <vcg/Utility.h>
#include <vcg/Point3.h>

namespace vcg {

template <class FLTYPE> class Sphere3
{
  public:
	Point3<FLTYPE> c;
	FLTYPE r;
	inline Sphere3( void ){}
	inline Sphere3(Point3<FLTYPE> const & cc,FLTYPE const & rr){c=cc;r=rr;}
	inline Sphere3(Point3<FLTYPE> const & cc){c=cc;r=0;}
	
	inline int Circumscribe(Point3<FLTYPE> &v0,Point3<FLTYPE> &v1,Point3<FLTYPE> &v2,Point3<FLTYPE> &v3);
	bool operator < (Sphere3 const & s) const {
		if(c!=s.c) return c<s.c;
		else return r<s.r;}
	bool operator == (Sphere3 const & s) const{
		return (c==s.c && r==s.r);}


	inline bool Intersect(Point3<FLTYPE> const & p) const {
		if((c-p).Norm2() < r*r) return true;
		else return false;
	}

	inline bool Intersect(Sphere3<FLTYPE> const & s) const {
		if((c-s.c).Norm()< r+s.r) return true;
		else return false;
	}

	inline FLTYPE Distance( Sphere3<FLTYPE> const & s ) const{
			return (c-s.c).Norm()-(r+s.r);
	}
	// It is not faster that Distance!!!
		inline FLTYPE SquaredDistance( Sphere3<FLTYPE> 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<FLTYPE> const &v0,Point3<FLTYPE> const &v1,Point3<FLTYPE> const &v2,Point3<FLTYPE> const &v3) 
{
  // Egdes (nota basterebbe calcolarne solo 4...)
  Point3<FLTYPE> e10=v1-v0;
  Point3<FLTYPE> e20=v2-v0;
  Point3<FLTYPE> e30=v3-v0;
  Point3<FLTYPE> e21=v2-v1;
  Point3<FLTYPE> e31=v3-v1;

  // Face Normals
  Point3<FLTYPE> n0 = e21 ^ e31;
  Point3<FLTYPE> n1 = e30 ^ e20;
  Point3<FLTYPE> n2 = e10 ^ e30;
  Point3<FLTYPE> 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 <class FLTYPE>
inline FLTYPE Distance( Sphere3<FLTYPE> const & s,Point3<FLTYPE> const & p ){
    return Norm(s.c-p)-s.r;
}
template <class FLTYPE>
inline FLTYPE SquaredDistance( Sphere3<FLTYPE> const & s,Point3<FLTYPE> const & p ){
    return SquaredNorm(s.c-p)-s.r*s.r;
}

template <class FLTYPE>
inline FLTYPE Distance( Sphere3<FLTYPE> const & s1,Sphere3<FLTYPE> const & s2 ){
    return Norm(s1.c-s2.c)-(s1.r+s2.r);
}
template <class FLTYPE>
inline FLTYPE SquaredDistance( Sphere3<FLTYPE> const & s,Sphere3<FLTYPE> 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 <class FLTYPE>
inline Sphere3<FLTYPE> MinSphere2 (Point3<FLTYPE> const &p0, Point3<FLTYPE> const &p1)
{
    Sphere3<FLTYPE> 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 <class FLTYPE>
inline Sphere3<FLTYPE> MinSphere2 (Point3<FLTYPE> const &p0, Point3<FLTYPE> const &p1, Point3<FLTYPE> 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<FLTYPE> minimal;

	Point3<FLTYPE> A( p0.v[0]-p2.v[0], p0.v[1]-p2.v[1], p0.v[2]-p2.v[2]);
	Point3<FLTYPE> 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<FLTYPE> 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<short>  Sphere3s;
typedef Sphere3<int>	  Sphere3i;
typedef Sphere3<float>  Sphere3f;
typedef Sphere3<double> Sphere3d;

} // end namespace
#endif