void IFXVector4::IncorporateSphere(const IFXVector4 * pBound)
{
	//* In addition to guarding against NULL incomming bounds, we need to
	//* check that the incomming bound is valid(radius != -1).
/*	IFXVector4 left, right;
	right.set(pBound->X(),pBound->Y(),pBound->Z(),pBound->Radius());
	left.set(X(),Y(),Z(),Radius());
	left.Incorporate(&right);
	m_value[0] = left.X();
	m_value[1] = left.Y();
	m_value[2] = left.Z();
	m_value[3] = left.Radius();

*/
	if(pBound->Radius() >= 0.0)
	{
		//* If THIS bound is uninitialized, then simply assimilate the values
		//* from the incomming sphere's bound.
		if(Radius()<0.0f)
		{
			*this=*pBound;
		}
		else
		{
			//* If initialized, determine the correct bounding sphere.

			//* First determine the vector between the spheres centers.
			IFXVector4 vectorBetweenCenters;
			vectorBetweenCenters.Subtract(*this,*pBound);

			//* distance between the spheres' centers
			F32 distanceBetweenCenters=vectorBetweenCenters.CalcMagnitude3();

			//* If this bound does not already encompass the incoming sphere.
			if(Radius() < pBound->Radius()+distanceBetweenCenters)
			{
				//* If the incoming sphere encompasses this sphere, then
				//* simply assimilate the values from the incomming sphere's
				//* bound.
				if(pBound->Radius() > Radius()+distanceBetweenCenters)
				{
					*this=*pBound;
				}
				//* Guard against divide by zero.
				else if(distanceBetweenCenters != 0.0f)
				{
					//* A new sphere must be calculated.

					F32 d=1.0f/distanceBetweenCenters;
					d*=Radius()-pBound->Radius();
					vectorBetweenCenters.Scale3(0.5f+0.5f*d);
					this->Add(*pBound,vectorBetweenCenters);
					m_value[3]=0.5f*
							(Radius()+pBound->Radius()+distanceBetweenCenters);
				}
			}
		}
	}
}

bool CSnork::trace_geometry(const Fvector &d, float &range)
{
	Fvector				dir;
	float				h, p;

	Fvector				Pl,Pc,Pr;
	Fvector				center;
	Center				(center);

	range				= trace (d);
	if (range > TRACE_RANGE) return false;
	
	float angle			= asin(1.f / range);

	// trace center ray
	dir					= d;

	dir.getHP			(h,p);
	p					+= angle;
	dir.setHP			(h,p);
	dir.normalize_safe	();

	range				= trace (dir);
	if (range > TRACE_RANGE) return false;

	Pc.mad				(center, dir, range);

	// trace left ray
	Fvector				temp_p;
	temp_p.mad			(Pc, XFORM().i, Radius() / 2);
	dir.sub				(temp_p, center);
	dir.normalize_safe	();

	range				= trace (dir);
	if (range > TRACE_RANGE) return false;

	Pl.mad				(center, dir, range);

	// trace right ray
	Fvector inv			= XFORM().i; 
	inv.invert			();
	temp_p.mad			(Pc, inv, Radius() / 2);
	dir.sub				(temp_p, center);
	dir.normalize_safe	();

	range				= trace (dir);
	if (range > TRACE_RANGE) return false;

	Pr.mad				(center, dir, range);

	float				h1,p1,h2,p2;

	Fvector().sub(Pl, Pc).getHP(h1,p1);
	Fvector().sub(Pc, Pr).getHP(h2,p2);

	return (fsimilar(h1,h2,0.1f) && fsimilar(p1,p2,0.1f));
}

integer Cas2Shuttle(CASblock & buffer, ShuttlePar & block){
        /* Reading M1950 coordinates and velocities from CAs */
      const int foot_cm=30.48;
double Coo_M1950[3], Vel_M1950[3], q[4], Greenw_Phi, Vel_angle;
polar Geo,Solar;
Euler Euler_LVLH;
      Coo_M1950[0]=CASvalue(buffer,(4490),8) * foot_cm;
      Coo_M1950[1]=CASvalue(buffer,(4508),8) * foot_cm;
      Coo_M1950[2]=CASvalue(buffer,(4526),8) * foot_cm;

      Vel_M1950[0]=CASvalue(buffer,(4687),8) * foot_cm;
      Vel_M1950[1]=CASvalue(buffer,(4701),8) * foot_cm;
      Vel_M1950[2]=CASvalue(buffer,(4715),8) * foot_cm;

      q[0]=CASvalue(buffer,(3291),8);
      q[1]=CASvalue(buffer,(3305),8);
      q[2]=CASvalue(buffer,(3319),8);
      q[3]=CASvalue(buffer,(3333),8);
      double times= CAStime(buffer,10);                
      time_t utime = time_t(times+0.5) + Year1998;      
      // check record
       if((Radius(Coo_M1950)<=.1) || (Radius(Vel_M1950)<=.1) || (times<=0))return 0;

     // convert here 
      Coordinates(Coo_M1950,Vel_M1950,q,utime,&Geo,&Vel_angle,&Euler_LVLH,&Solar,&Greenw_Phi);
      if(!td2f(Geo.Teta))return -1;
      if(!td2f(Geo.Phi))return -1;
      if(!td2f(Geo.R))return -1;
      if(!td2f(Greenw_Phi))return -1;
      if(!td2f(Euler_LVLH.Yaw))return -1;
      if(!td2f(Euler_LVLH.Pitch))return -1;
      if(!td2f(Euler_LVLH.Yaw))return -1;
      if(!td2f(Vel_angle))return -1;
      if(!td2f(Solar.R))return -1;
      if(!td2f(Solar.Phi))return -1;
      if(!td2f(Solar.Teta))return -1;

      block.StationTheta=d2f(Geo.Teta);
      block.StationPhi=d2f(Geo.Phi);
      block.StationR=d2f(Geo.R);
      block.GrMedPhi=d2f(Greenw_Phi);
      block.StationYaw=d2f(Euler_LVLH.Yaw);
      block.StationPitch=d2f(Euler_LVLH.Pitch);  
      block.StationRoll=d2f(  Euler_LVLH.Roll);
      block.StationSpeed=d2f(  Vel_angle);
      block.SunR=d2f(Solar.R);
      block.SunPhi=d2f(Solar.Phi);
      block.SunTheta=d2f(Solar.Teta);
      //cout <<" "<<ClockTune(utime)<<" "<<ctime(&utime)<<endl;
      block.Time=utime-ClockTune(utime);
}

bool Cylinder::SetData(const std::string &strDataType, const std::string &strValue, bool bThrowError)
{
	std::string strType = Std_CheckString(strDataType);

	if(RigidBody::SetData(strType, strValue, false))
		return true;

	if(strType == "RADIUS")
	{
		Radius((float) atof(strValue.c_str()));
		return true;
	}

	if(strType == "HEIGHT")
	{
		Height((float) atof(strValue.c_str()));
		return true;
	}

	if(strType == "SIDES")
	{
		Sides(atoi(strValue.c_str()));
		return true;
	}

	//If it was not one of those above then we have a problem.
	if(bThrowError)
		THROW_PARAM_ERROR(Al_Err_lInvalidDataType, Al_Err_strInvalidDataType, "Data Type", strDataType);

	return false;
}

void Smooth::SeedRing() {
   for (int x=0;x<sizex;x++) {
     for (int y=0;y<sizey;y++) {
      (*field)[x][y]=Ring(Radius(0,0,x,y));
     }
   }
}

void CMosquitoBald::Affect(SZoneObjectInfo* O) 
{
	CPhysicsShellHolder *pGameObject = smart_cast<CPhysicsShellHolder*>(O->object);
	if(!pGameObject) return;

	if(O->zone_ignore) return;

	Fvector P; 
	XFORM().transform_tiny(P,CFORM()->getSphere().P);

	Fvector hit_dir; 
	hit_dir.set(	::Random.randF(-.5f,.5f), 
					::Random.randF(.0f,1.f), 
					::Random.randF(-.5f,.5f)); 
	hit_dir.normalize();

	Fvector position_in_bone_space;

	VERIFY(!pGameObject->getDestroy());

	float dist = pGameObject->Position().distance_to(P) - pGameObject->Radius();
	float power = Power(dist>0.f?dist:0.f, Radius());
	float impulse = m_fHitImpulseScale*power*pGameObject->GetMass();

	if(power > 0.01f) 
	{
		position_in_bone_space.set(0.f,0.f,0.f);
		CreateHit(pGameObject->ID(),ID(),hit_dir,power,0,position_in_bone_space,impulse,m_eHitTypeBlowout);
		PlayHitParticles(pGameObject);
	}
}

void CircleSeg<D> :: GetCoeff (Vector & coeff) const
{ 
  coeff[0] = coeff[1] = 1.0;
  coeff[2] = 0.0;
  coeff[3] = -2.0 * pm[0];
  coeff[4] = -2.0 * pm[1];
  coeff[5] = sqr(pm[0]) + sqr(pm[1]) - sqr(Radius());
}

double f22(double k,void * params)     /* No integral before the deltaM*/
 {
double alpha = *(double *) params;
double f1 = pow(k,2.0) / (2 * pow(M_PI,2.0));
double f2 = Pk( k) * pow(Windowfunc( k,Radius(alpha)),2.0) ;
double f = f1 * f2;
return sqrt(f);
}

filling Smooth::FillingRing(int x, int y) const {
  double total=0.0;
  for (int x1=0;x1<sizex;x1++) {
     for (int y1=0;y1<sizey;y1++) {
       total+=(*field)[x1][y1]*Ring(Radius(x,y,x1,y1));
    }
  };
  return total/normalisation_ring;
}

double Smooth::NormalisationRing() const {
  double total=0.0;
  for (int x=0;x<sizex;x++) {
     for (int y=0;y<sizey;y++) {
       total+=Ring(Radius(0,0,x,y));
    }
  };
  return total;
}

bool
Graphic::CheckVisibility(Projector& projector)
{
    if (projector.IsVisible(     Location(), Radius()) &&
            projector.ApparentRadius(Location(), Radius()) > 1) {

        visible        = true;
    }
    else {
        visible        = false;
        screen_rect.x  = 2000;
        screen_rect.y  = 2000;
        screen_rect.w  = 0;
        screen_rect.h  = 0;
    }
    
    return visible;
}

Point<D> CircleSeg<D> :: GetPoint (double t) const
{
  if (t >= 1.0)  { return p3; }
     
  double phi = StartAngle() + t*(EndAngle()-StartAngle());
  Vec<D>  tmp(cos(phi),sin(phi));
     
  return pm + Radius()*tmp;
}

void CBaseGraviZone::AffectPullAlife(CEntityAlive* EA,const Fvector& throw_in_dir,float dist)
{
	float rel_power = RelativePower(dist, Radius());
	float throw_power = m_fThrowInImpulseAlive*rel_power*rel_power*rel_power*rel_power*rel_power;

	Fvector vel;
	vel.set(throw_in_dir);
	vel.mul(throw_power);
	EA->character_physics_support()->movement()->AddControlVel(vel);
}

void Cylinder::Load(CStdXml &oXml)
{
	RigidBody::Load(oXml);

	oXml.IntoElem();  //Into RigidBody Element
	Radius(oXml.GetChildFloat("Radius", m_fltRadius));
	Height(oXml.GetChildFloat("Height"), m_fltHeight);
	Sides(oXml.GetChildInt("Sides", m_iSides));
	oXml.OutOfElem(); //OutOf RigidBody Element
}

// ==============================================================================================================================================
//
double Orbit::IntersectionInterpolate(Orbit *tgt,double start,double end,bool dir)
{
	int i;
	bool ok=false;
	double dif;
	double gamma,sector,aa,da,db,first=0;
	VECTOR3 v;

	gamma=start;
	sector=end;

	if (dir) sector=-sector;

	sector/=2;

	v=tgt->Position(gamma);
	Longitude(v,&db,NULL,&aa);
	da=Radius(aa);
	first=da-db;

	gamma+=sector;

	for (i=0;i<30;i++) {  
	      
		gamma=limit(gamma);
		v=tgt->Position(gamma);
		Longitude(v,&db,NULL,&aa);
		da=Radius(aa);
		dif=da-db;
	
		if ((first*dif)<0) sector=-sector, ok=true;
		first=dif;
		
		sector/=2;
		gamma+=sector;
        
	}

	if (!ok) return -1.0;

	return(tgt->Translate(this,limit(gamma)));
}

void R3Cylinder::
Draw(const R3DrawFlags draw_flags) const
{
#if (RN_3D_GRFX == RN_OPENGL)
    // Create GLU quadric
    static GLUquadricObj *cylinder = gluNewQuadric();

    // Push matrix
    R4Matrix matrix = R4identity_matrix;
    matrix.Translate(axis.Start().Vector());
    matrix.Rotate(R3posz_vector, axis.Vector());
    matrix.Push();

    // Draw surface
    if (draw_flags[R3_SURFACES_DRAW_FLAG]) {
	if (draw_flags[R3_VERTEX_NORMALS_DRAW_FLAG]) gluQuadricNormals(cylinder, (GLenum) GLU_SMOOTH);
	else if (draw_flags[R3_SURFACE_NORMALS_DRAW_FLAG]) gluQuadricNormals(cylinder, (GLenum) GLU_FLAT);
	if (draw_flags[R3_VERTEX_TEXTURE_COORDS_DRAW_FLAG]) gluQuadricTexture(cylinder, GL_TRUE);
	else gluQuadricTexture(cylinder, GL_FALSE);
	gluQuadricDrawStyle(cylinder, (GLenum) GLU_FILL);
	gluCylinder(cylinder, base.Radius(), top.Radius(), Height(), 16, 1);
	gluQuadricOrientation(cylinder, (GLenum) GLU_INSIDE);
	gluDisk(cylinder, 0.0, Radius(), 16, 1);
	gluQuadricOrientation(cylinder, (GLenum) GLU_OUTSIDE);
	glTranslated(0.0, 0.0, Height());
	gluDisk(cylinder, 0.0, Radius(), 16, 1);
    }

    // Draw edges
    if (draw_flags[R3_EDGES_DRAW_FLAG]) {
	gluQuadricNormals(cylinder, (GLenum) GLU_NONE);
	gluQuadricTexture(cylinder, GL_FALSE);
	gluQuadricDrawStyle(cylinder, (GLenum) GLU_SILHOUETTE);
	gluCylinder(cylinder, base.Radius(), top.Radius(), Height(), 16, 1);
    }

    // Pop matrix
    matrix.Pop();
#else
    RNAbort("Not Implemented");
#endif
}

//////////////////////////////////////////////////////////////////////////
//! build a sphere
//////////////////////////////////////////////////////////////////////////
void buildSphere(void)
{
    Real32 Radius(frand()*1.5+0.2);
    Matrix m;
    //create OpenSG mesh
    GeometryPtr sphere;
    NodePtr sphereNode = makeSphere(2, Radius);
    sphere = GeometryPtr::dcast(sphereNode->getCore());
    SimpleMaterialPtr sphere_mat = SimpleMaterial::create();
    beginEditCP(sphere_mat);
    sphere_mat->setAmbient(Color3f(0.0,0.0,0.0));
    sphere_mat->setDiffuse(Color3f(0.0,0.0,1.0));
    endEditCP(sphere_mat);
    beginEditCP(sphere, Geometry::MaterialFieldMask);
    sphere->setMaterial(sphere_mat);
    endEditCP(sphere);
    TransformPtr sphereTrans;
    NodePtr sphereTransNode = makeCoredNode<Transform>(&sphereTrans);
    m.setIdentity();
    Real32 randX = frand()*10.0-5.0;
    Real32 randY = frand()*10.0-5.0;
    m.setTranslate(randX, randY, 10.0);
    beginEditCP(sphereTrans, Transform::MatrixFieldMask);
    sphereTrans->setMatrix(m);
    endEditCP(sphereTrans);
    //create ODE data
    PhysicsBodyPtr sphereBody = PhysicsBody::create(physicsWorld);
    beginEditCP(sphereBody, PhysicsBody::PositionFieldMask | PhysicsBody::AngularDampingFieldMask);
        sphereBody->setPosition(Vec3f(randX, randY, 10.0));
        sphereBody->setAngularDamping(0.0001);
    endEditCP(sphereBody, PhysicsBody::PositionFieldMask | PhysicsBody::AngularDampingFieldMask);
    sphereBody->setSphereMass(0.4,Radius);

    PhysicsSphereGeomPtr sphereGeom = PhysicsSphereGeom::create();
    beginEditCP(sphereGeom, PhysicsSphereGeom::BodyFieldMask | PhysicsSphereGeom::SpaceFieldMask | PhysicsSphereGeom::RadiusFieldMask | PhysicsSphereGeom::CategoryBitsFieldMask);
        sphereGeom->setBody(sphereBody);
        sphereGeom->setSpace(physicsSpace);
        sphereGeom->setRadius(Radius);
        sphereGeom->setCategoryBits(SphereCategory);
    endEditCP(sphereGeom, PhysicsSphereGeom::BodyFieldMask | PhysicsSphereGeom::SpaceFieldMask | PhysicsSphereGeom::RadiusFieldMask | PhysicsSphereGeom::CategoryBitsFieldMask);
    
    //add attachments
    beginEditCP(sphereNode, Node::AttachmentsFieldMask);
    sphereNode->addAttachment(sphereGeom);
    endEditCP(sphereNode);
    beginEditCP(sphereTransNode, Node::AttachmentsFieldMask | Node::ChildrenFieldMask);
    sphereTransNode->addAttachment(sphereBody);
    sphereTransNode->addChild(sphereNode);
    endEditCP(sphereTransNode);
    //add to SceneGraph
    beginEditCP(spaceGroupNode, Node::ChildrenFieldMask);
    spaceGroupNode->addChild(sphereTransNode);
    endEditCP(spaceGroupNode);
}

//////////////////////////////////////////////////////////////////////////
//! build a ship
//////////////////////////////////////////////////////////////////////////
PhysicsBodyRefPtr buildShip(Vec3f Dimensions, Pnt3f Position)
{
    Real32 Radius(osgMax(Dimensions.x(), Dimensions.y())/2.0f);
    Real32 Length(Dimensions.z() - 2.0f*Radius);

    Matrix m;
    //create OpenSG mesh
    GeometryRefPtr box;
    NodeRefPtr boxNode = makeBox(Dimensions.x(), Dimensions.y(), Dimensions.z(), 1, 1, 1);
    box = dynamic_cast<Geometry*>(boxNode->getCore());
    SimpleMaterialRefPtr box_mat = SimpleMaterial::create();
        box_mat->setAmbient(Color3f(0.0,0.0,0.0));
        box_mat->setDiffuse(Color3f(1.0,1.0 ,0.0));
        box->setMaterial(box_mat);
    TransformRefPtr boxTrans;
    NodeRefPtr boxTransNode = makeCoredNode<Transform>(&boxTrans);
    m.setIdentity();
    m.setTranslate(Position - Vec3f(0.0f,0.0f,0.5f*Dimensions.z()));
        boxTrans->setMatrix(m);

        for(UInt32 i(0) ; i<box->getPositions()->size() ; ++i)
        {
            box->getPositions()->setValue<Pnt3f>(box->getPositions()->getValue<Pnt3f>(i) + Vec3f(0.0,0.0,Dimensions.z()/2.0f),i);
        }


    //create ODE data

    PhysicsBodyRefPtr CapsuleBody = PhysicsBody::create(physicsWorld);
        CapsuleBody->setPosition(Vec3f(Position - Vec3f(0.0f,0.0f,0.5f*Dimensions.z())));
        CapsuleBody->setLinearDamping(0.01);
        CapsuleBody->setMaxAngularSpeed(0.0);
    CapsuleBody->setCapsuleMass(1.0,3,Radius, Length);

    PhysicsCapsuleGeomRefPtr CapsuleGeom = PhysicsCapsuleGeom::create();
        CapsuleGeom->setBody(CapsuleBody);
        CapsuleGeom->setOffsetPosition(Vec3f(0.0f,0.0f,0.5f*Dimensions.z()));
        CapsuleGeom->setSpace(hashSpace);
        CapsuleGeom->setRadius(Radius);
        CapsuleGeom->setLength(Length);

    //add attachments
        boxNode->addAttachment(CapsuleGeom);
        boxTransNode->addAttachment(CapsuleBody);
        boxTransNode->addChild(boxNode);

    //add to SceneGraph
        spaceGroupNode->addChild(boxTransNode);

    commitChanges();

    return CapsuleBody;
}

void SWIFT_Object::Initialize( SWIFT_Tri_Mesh* m, bool f, bool uc,
                               SWIFT_Real ber, SWIFT_Real bea, bool cp )
{
    mesh = m;

    fixed = f;
    cube = uc;
    enlargement = ber * Radius() + bea;

    if( uc ) {
        // Create the cube
        radius = Radius() + enlargement;
    } else {
        // Initialize the dynamic bounding box
        min_es[0] = min_es[1] = min_es[2] = max_es[0] = 
        max_es[1] = max_es[2] = mesh->Root()->Other_Faces().Empty() ?
                                    mesh->Root()->Faces()[0].Edge1P() :
                                    mesh->Root()->Other_Faces()[0]->Edge1P();
    }
    Update_Boxes();
}

ON_3dPoint ON_Circle::ClosestPointTo( const ON_3dPoint& point ) const
{
  ON_3dPoint P;
  ON_3dVector V = plane.ClosestPointTo( point ) - Center();
  if ( V.Unitize() ) {
    V.Unitize();
    P = Center() + Radius()*V;
  }
  else {
    P = PointAt(0.0);
  }
  return P;
}