e595dbc88e
This was SVN commit r5393.
178 lines
5.3 KiB
C++
178 lines
5.3 KiB
C++
#include "precompiled.h"
|
|
# include <math.h>
|
|
# include "sr_box.h"
|
|
# include "sr_line.h"
|
|
# include "sr_input.h"
|
|
# include "sr_output.h"
|
|
|
|
//===========================================================================
|
|
|
|
// Important: Static initializations cannot use other static initialized
|
|
// variables ( as SrVec::i, ... ), as we cannot know the order that they
|
|
// will be initialized by the compiler.
|
|
|
|
const SrLine SrLine::x ( SrVec(0,0,0), SrVec(1.0f,0,0) );
|
|
const SrLine SrLine::y ( SrVec(0,0,0), SrVec(0,1.0f,0) );
|
|
const SrLine SrLine::z ( SrVec(0,0,0), SrVec(0,0,1.0f) );
|
|
|
|
//============================== SrLine ====================================
|
|
|
|
#define EPSILON 0.00001 // floats have 7 decimals
|
|
|
|
// Original code from : http://www.acm.org/jgt/papers/MollerTrumbore97/
|
|
bool SrLine::intersects_triangle ( const SrVec &v0, const SrVec &v1, const SrVec &v2,
|
|
float &t, float &u, float &v ) const
|
|
{
|
|
SrVec dir, edge1, edge2, tvec, pvec, qvec;
|
|
float det, inv_det;
|
|
|
|
dir = p2 - p1;
|
|
edge1 = v1 - v0; // find vectors for two edges sharing v0
|
|
edge2 = v2 - v0;
|
|
pvec = cross ( dir, edge2 ); // begin calculating determinant - also used to calculate U parameter
|
|
det = dot ( edge1, pvec ); // if determinant is near zero, ray lies in plane of triangle
|
|
// printf("det=%f\n",det);
|
|
|
|
if ( SR_NEXTZ(det,EPSILON) )
|
|
{ //sr_out.warning("det in ray_triangle fails => %f",(float)det);
|
|
return false;
|
|
}
|
|
inv_det = 1.0f / det;
|
|
|
|
tvec = p1 - v0; // calculate distance from v0 to ray origin
|
|
u = dot(tvec, pvec) * inv_det; // calculate U parameter and test bounds
|
|
if ( u<0.0 || u>1.0 ) return false;
|
|
|
|
qvec = cross ( tvec, edge1 ); // prepare to test V parameter
|
|
v = dot(dir, qvec) * inv_det; // calculate V parameter and test bounds
|
|
if ( v<0.0 || u+v>1.0 ) return false;
|
|
t = dot(edge2,qvec) * inv_det; // calculate t, ray intersects triangle
|
|
|
|
return true;
|
|
}
|
|
|
|
bool SrLine::intersects_square ( const SrVec &v1, const SrVec &v2,
|
|
const SrVec &v3, const SrVec &v4, float& t ) const
|
|
{
|
|
float u, v;
|
|
if ( intersects_triangle ( v1, v2, v3, t, u, v ) ) return true;
|
|
if ( intersects_triangle ( v1, v3, v4, t, u, v ) ) return true;
|
|
return false;
|
|
}
|
|
|
|
int SrLine::intersects_box ( const SrBox& box, float& t1, float& t2, SrVec* vp ) const
|
|
{
|
|
SrVec p1, p2, p3, p4, p;
|
|
float t[6];
|
|
int side[6];
|
|
int tsize=0;
|
|
|
|
# define INTERSECT(s) if ( intersects_square(p1,p2,p3,p4,t[tsize]) ) { side[tsize]=s; tsize++; }
|
|
|
|
box.get_side ( p1, p2, p3, p4, 0 );
|
|
INTERSECT(0);
|
|
box.get_side ( p1, p2, p3, p4, 1 );
|
|
INTERSECT(1);
|
|
box.get_side ( p1, p2, p3, p4, 2 );
|
|
INTERSECT(2);
|
|
box.get_side ( p1, p2, p3, p4, 3 );
|
|
INTERSECT(3);
|
|
box.get_side ( p1, p2, p3, p4, 4 );
|
|
INTERSECT(4);
|
|
box.get_side ( p1, p2, p3, p4, 5 );
|
|
INTERSECT(5);
|
|
|
|
# undef INTERSECT
|
|
|
|
if ( tsize==0 )
|
|
{ t1=t2=0; }
|
|
else if ( tsize==1 )
|
|
{ t1=t2=t[0]; }
|
|
else if ( tsize==2 )
|
|
{ float tmpf;
|
|
int tmpi;
|
|
if ( t[1]<t[0] ) { SR_SWAPT(t[0],t[1],tmpf); SR_SWAPT(side[0],side[1],tmpi); }
|
|
t1 = t[0];
|
|
t2 = t[1];
|
|
}
|
|
else // sort according to t and take the two extremes
|
|
{ int i, j, tmpi;
|
|
float tmpf;
|
|
for ( i=0; i<tsize; i++ )
|
|
for ( j=i; j<tsize; j++ )
|
|
if ( t[j]<t[i] ) { SR_SWAPT(t[i],t[j],tmpf); SR_SWAPT(side[i],side[j],tmpi); }
|
|
t1 = t[0];
|
|
t2 = t[tsize-1];
|
|
tsize = 2;
|
|
}
|
|
|
|
if (tsize>0 && vp) box.get_side ( vp[0], vp[1], vp[2], vp[3], side[0] );
|
|
return tsize;
|
|
}
|
|
|
|
int SrLine::intersects_sphere ( const SrPnt& center, float radius, SrPnt* vp ) const
|
|
{
|
|
// set up quadratic Q(t) = a*t^2 + 2*b*t + c
|
|
SrVec dir = p2-p1;
|
|
SrVec kdiff = p1 - center;
|
|
float a = dir.norm2();
|
|
float b = dot ( kdiff, dir );
|
|
float c = kdiff.norm2() - radius*radius;
|
|
|
|
float aft[2];
|
|
float discr = b*b - a*c;
|
|
|
|
if ( discr < 0.0f )
|
|
{ return 0;
|
|
}
|
|
else if ( discr > 0.0f )
|
|
{ float root = sqrtf(discr);
|
|
float inva = 1.0f/a;
|
|
aft[0] = (-b - root)*inva;
|
|
aft[1] = (-b + root)*inva;
|
|
if ( vp )
|
|
{ vp[0] = p1 + aft[0]*dir;
|
|
vp[1] = p1 + aft[1]*dir;
|
|
if ( dist2(vp[1],p1)<dist2(vp[0],p1) )
|
|
{ SrPnt tmp;
|
|
SR_SWAP(vp[0],vp[1]);
|
|
}
|
|
}
|
|
return 2;
|
|
}
|
|
else
|
|
{ aft[0] = -b/a;
|
|
if ( vp ) vp[0] = p1 + aft[0]*dir;
|
|
return 1;
|
|
}
|
|
}
|
|
|
|
//http://astronomy.swin.edu.au/~pbourke/geometry/pointline/source.c
|
|
SrPnt SrLine::closestpt ( SrPnt p, float* k ) const
|
|
{
|
|
SrVec v (p2-p1);
|
|
|
|
float u = ( ( (p.x-p1.x) * (v.x) ) +
|
|
( (p.y-p1.y) * (v.y) ) +
|
|
( (p.z-p1.z) * (v.z) ) ) / ( v.norm2() );
|
|
|
|
//if( u<0.0f || u>1.0f ) // closest point does not fall within the line segment
|
|
if ( k ) *k=u;
|
|
|
|
return p1 + u*v;
|
|
}
|
|
|
|
//============================== friends ====================================
|
|
|
|
SrOutput& operator<< ( SrOutput& o, const SrLine& l )
|
|
{
|
|
return o << l.p1 <<" "<< l.p2;
|
|
}
|
|
|
|
SrInput& operator>> ( SrInput& in, SrLine& l )
|
|
{
|
|
return in >> l.p1 >> l.p2;
|
|
}
|
|
|
|
//============================= End of File ===========================================
|