static char solve2pt(int ind,int inda,int indb,float ha,float hb,char ch)
{
int kmin /* ,i,k ,i1,k1 */;
float x1,x2;
float xa,ta,xb,tb,x,t,tmin;
float d,dp,dp2,df;
char /* ch1='y', */ ch2='n';
int ntry=0;
xa=*(x0+inda);
ta=*(t0+inda);
xb=*(x0+indb);
tb=*(t0+indb);
tmin=SF_MAX(ta,tb)-TOL;
kmin=floor(tmin/ht);
x1=SF_MIN(xa,xb)-TOL;
x2=SF_MAX(xa,xb)+TOL;
t=*(t0+ind);
if( kmin<nt1 ) {
while( ch2=='n' && ntry<NTRYMAX ) {
x=lateral(t,xa,xb,ta,tb,ha,hb);
/*i=SF_MAX(0,SF_MIN(nx1-1,floor(x/hx)));
k=SF_MAX(0,SF_MIN(nt1-1,floor(t/ht))); */
d=fn(t,ta,tb,xa,xb,ha,hb);
dp=2.0*d;
dp2=dp;
while( fabs(d)>TOL && fabs(dp2)>fabs(d) ) {
df=dfn(t,ta,tb,xa,xb,ha,hb);
t-=d/df;
dp2=dp;
dp=d;
d=fn(t,ta,tb,xa,xb,ha,hb);
}
x=lateral(t,xa,xb,ta,tb,ha,hb);
/* i1=SF_MAX(0,SF_MIN(nx1-1,floor(x/hx)));
k1=SF_MAX(0,SF_MIN(nt1-1,floor(t/ht))); */
if( x>=x1 && x<=x2 && t>=tmin ) ch2='y';
ntry++;
}
if( fabs(d)<=TOL && t>=tmin ) {
if( x>=x1 && x<=x2 ) {
/* ch1='n'; */
if( *(pup+ind)<=1 || (*(pup+ind)==2 && t<(*(t0+ind))) ) {
ch='s';
*(t0+ind)=t;
*(x0+ind)=x;
*(pup+ind)=2;
*(v+ind)=1.0/linterp2(x,t);
}
ch=(ch=='s') ? 's' : 'n';
}
}
}
else
ch=(ch=='s') ? 's' : 'f';
return ch;
}
static float fn(float t,float ta,float tb,float xa,float xb,
float ha,float hb) {
float dta,dtb,x,ss;
dta=(t-ta)/ha;
dtb=(t-tb)/hb;
x=lateral(t,xa,xb,ta,tb,ha,hb);
ss=linterp2(x,t);
return (dta*dta+dtb*dtb)-ss*ss;
}
osg::Geode *createTorus(float innerRadius, float outerRadius, float sweepCuts, float sphereCuts)
{
osg::ref_ptr<osg::Geode> geode = new osg::Geode;
bool create_body = true;
if (create_body) {
float inner = innerRadius;
float outer = outerRadius;
float tube_radius = (outer - inner) * 0.5;
float avg_center = (inner + outer) * 0.5;
float start_sweep = 0; // this->getStartSweep();
float end_sweep = osg::DegreesToRadians(360.0); // this->getEndSweep();
int torus_sweeps = sweepCuts;
int sphere_sweeps = sphereCuts;
float dsweep = (end_sweep - start_sweep) / (float)torus_sweeps;
float dphi = osg::DegreesToRadians(360.0) / (float)sphere_sweeps;
for (int j = 0; j < sphere_sweeps; j++) {
osg::Vec3Array *vertices = new osg::Vec3Array;
osg::Vec3Array *normals = new osg::Vec3Array;
float phi = dphi * (float)j;
float cosPhi = cosf(phi);
float sinPhi = sinf(phi);
float next_cosPhi = cosf(phi + dphi);
float next_sinPhi = sinf(phi + dphi);
float z = tube_radius * sinPhi;
float yPrime = avg_center + tube_radius * cosPhi;
float next_z = tube_radius * next_sinPhi;
float next_yPrime = avg_center + tube_radius * next_cosPhi;
float old_x = yPrime * cosf(-dsweep);
float old_y = yPrime * sinf(-dsweep);
float old_z = z;
for (int i = 0; i < torus_sweeps; ++i) {
float sweep = start_sweep + dsweep * i;
float cosSweep = cosf(sweep);
float sinSweep = sinf(sweep);
float x = yPrime * cosSweep;
float y = yPrime * sinSweep;
float next_x = next_yPrime * cosSweep;
float next_y = next_yPrime * sinSweep;
vertices->push_back(osg::Vec3(next_x, next_y, next_z));
vertices->push_back(osg::Vec3(x, y, z));
// calculate normals
osg::Vec3 lateral(next_x - x, next_y - y, next_z - z);
osg::Vec3 longitudinal(x - old_x, y - old_y, z - old_z);
osg::Vec3 normal = longitudinal ^ lateral; // cross product
normal.normalize();
normals->push_back(normal);
normals->push_back(normal);
old_x = x;
old_y = y;
old_z = z;
} // end torus loop
// the last point
float last_sweep = start_sweep + end_sweep;
float cosLastSweep = cosf(last_sweep);
float sinLastSweep = sinf(last_sweep);
float x = yPrime * cosLastSweep;
float y = yPrime * sinLastSweep;
float next_x = next_yPrime * cosLastSweep;
float next_y = next_yPrime * sinLastSweep;
vertices->push_back(osg::Vec3(next_x, next_y, next_z));
vertices->push_back(osg::Vec3(x, y, z));
osg::Vec3 lateral(next_x - x, next_y - y, next_z - z);
osg::Vec3 longitudinal(x - old_x, y - old_y, z - old_z);
osg::Vec3 norm = longitudinal ^ lateral;
norm.normalize();
normals->push_back(norm);
normals->push_back(norm);
osg::ShadeModel *shademodel = new osg::ShadeModel;
shademodel->setMode(osg::ShadeModel::SMOOTH);
osg::StateSet *stateset = new osg::StateSet;
stateset->setAttribute(shademodel);
osg::ref_ptr<osg::Geometry> geometry = new osg::Geometry;
geometry->setStateSet(stateset);
geometry->setVertexArray(vertices);
osg::Vec4Array *colors = new osg::Vec4Array;
colors->push_back(osg::Vec4(1.0, 1.0, 0.0, 1.0)); // this->getColor());
geometry->setColorArray(colors);
geometry->setColorBinding(osg::Geometry::BIND_OVERALL);
geometry->setNormalArray(normals);
geometry->setNormalBinding(osg::Geometry::BIND_PER_VERTEX);
geometry->addPrimitiveSet(
new osg::DrawArrays(osg::PrimitiveSet::QUAD_STRIP, 0,
vertices->size()));
geode->addDrawable(geometry.get());
} // end cirle loop
} // endif create_body
return geode.release();
}