OSGLine.cpp

Go to the documentation of this file.
00001 /*---------------------------------------------------------------------------*\
00002  *                                OpenSG                                     *
00003  *                                                                           *
00004  *                                                                           *
00005  *             Copyright (C) 2000-2003 by the OpenSG Forum                   *
00006  *                                                                           *
00007  *                            www.opensg.org                                 *
00008  *                                                                           *
00009  *   contact: dirk@opensg.org, gerrit.voss@vossg.org, jbehr@zgdv.de          *
00010  *                                                                           *
00011 \*---------------------------------------------------------------------------*/
00012 /*---------------------------------------------------------------------------*\
00013  *                                License                                    *
00014  *                                                                           *
00015  * This library is free software; you can redistribute it and/or modify it   *
00016  * under the terms of the GNU Library General Public License as published    *
00017  * by the Free Software Foundation, version 2.                               *
00018  *                                                                           *
00019  * This library is distributed in the hope that it will be useful, but       *
00020  * WITHOUT ANY WARRANTY; without even the implied warranty of                *
00021  * MERCHANTABILITY or FITNESS FOR A PARTICULAR PURPOSE.  See the GNU         *
00022  * Library General Public License for more details.                          *
00023  *                                                                           *
00024  * You should have received a copy of the GNU Library General Public         *
00025  * License along with this library; if not, write to the Free Software       *
00026  * Foundation, Inc., 675 Mass Ave, Cambridge, MA 02139, USA.                 *
00027  *                                                                           *
00028  *                                                                           *
00029 \*---------------------------------------------------------------------------*/
00030 /*---------------------------------------------------------------------------*\
00031  *                                Changes                                    *
00032  *                                                                           *
00033  *                                                                           *
00034  *                                                                           *
00035  *                                                                           *
00036  *                                                                           *
00037  *                                                                           *
00038 \*---------------------------------------------------------------------------*/
00039
00040 //---------------------------------------------------------------------------
00041 //  Includes
00042 //---------------------------------------------------------------------------
00043
00044 #include <cmath>
00045
00046 #include "OSGConfig.h"
00047
00048 #include <cassert>
00049
00050 #include "OSGLine.h"
00051
00052 #include "OSGPlane.h"
00053 #include "OSGBoxVolume.h"
00054 #include "OSGSphereVolume.h"
00055 #include "OSGCylinderVolume.h"
00056 #include "OSGFrustumVolume.h"
00057
00058 OSG_BEGIN_NAMESPACE
00059
00067 /*-------------------------- constructor ----------------------------------*/
00068
00069 Line::Line(void) :
00070     _pos(0.f, 0.f ,0.f),
00071     _dir(0.f, 0.f, 0.f)
00072 {
00073 }
00074
00075
00076 Line::Line(const Line &obj) :
00077     _pos(obj._pos),
00078     _dir(obj._dir)
00079 {
00080 }
00081
00082
00083 Line::Line(const Pnt3r &p0, const Pnt3r &p1) :
00084     _pos(0.f, 0.f ,0.f),
00085     _dir(0.f, 0.f, 0.f)
00086 {
00087     setValue(p0, p1);
00088 }
00089
00090
00091 Line::Line(const Pnt3r &pos, const Vec3r &dir) :
00092     _pos(pos),
00093     _dir(dir)
00094 {
00095     _dir.normalize();
00096 }
00097
00098
00099 /*--------------------------- destructor ----------------------------------*/
00100
00101 Line::~Line(void)
00102 {
00103 }
00104
00105 /*---------------------------------------------------------------------------*/
00106 /* Operators                                                                 */
00107
00108 Line &Line::operator =(const Line &rhs)
00109 {
00110     if(this == &rhs)
00111         return *this;
00112
00113     _pos = rhs._pos;
00114     _dir = rhs._dir;
00115
00116     return *this;
00117 }
00118
00119 bool Line::operator ==(const Line &rhs) const
00120 {
00121     return (_pos == rhs._pos) && (_dir == rhs._dir);
00122 }
00123
00124 /*------------------------------ feature ----------------------------------*/
00125
00126 void Line::setValue(const Pnt3r &p0, const Pnt3r &p1)
00127 {
00128     _pos = p0;
00129     _dir = p1 - p0;
00130
00131     _dir.normalize();
00132 }
00133
00134
00135 void Line::setValue(const Pnt3r &pos, const Vec3r &dir)
00136 {
00137     _pos = pos;
00138     _dir = dir;
00139     _dir.normalize();
00140 }
00141
00146 bool Line::getClosestPoints(const Line  &line2    ,
00147                                   Pnt3r &ptOnThis ,
00148                                   Pnt3r &ptOnLine2) const
00149 {
00150     // Assumes that _dir and line2._dir are valid and normalized
00151
00152     Vec3r normal=_dir.cross(line2._dir);
00153
00154     if(normal.isZero())
00155         return false; // Lines are parallel
00156
00157     Vec3r p0p1 = line2._pos - _pos;
00158
00159     Real lengthSqr = normal.squareLength();
00160     Real s         = p0p1.cross(line2._dir).dot(normal) / lengthSqr;
00161     Real t         = p0p1.cross(      _dir).dot(normal) / lengthSqr;
00162
00163     ptOnThis  =       _pos + s *       _dir;
00164     ptOnLine2 = line2._pos + t * line2._dir;
00165
00166     return true;
00167 }
00168
00172 Pnt3r Line::getClosestPoint(const Pnt3r &point) const
00173 {
00174     Vec3r vec(point - _pos);
00175
00176     return _pos + _dir * vec.dot(_dir);
00177 }
00178
00179
00183 Real Line::distance(const Pnt3r &point) const
00184 {
00185     return (point - getClosestPoint(point)).length();
00186 }
00187
00188
00189 /*-------------------------- intersection ---------------------------------*/
00190
00194 bool Line::intersect(const SphereVolume &sphere) const
00195 {
00196     Real ent;
00197     Real ex;
00198
00199     return this->intersect(sphere, ent, ex);
00200 }
00201
00205 bool Line::intersect(const SphereVolume &sphere,
00206                            Real         &enter,
00207                            Real         &exit  ) const
00208 {
00209     Vec3r v;
00210     Pnt3r center;
00211
00212     sphere.getCenter(center);
00213
00214     Real radius;
00215     Real h;
00216     Real b;
00217     Real d;
00218     Real t1;
00219     Real t2;
00220
00221     radius = sphere.getRadius();
00222
00223     v = center - _pos;
00224
00225     h = (v.dot(v))-(radius * radius);
00226     b = (v.dot(_dir));
00227
00228     if(h >= 0.f && b <= 0.f)
00229         return false;
00230
00231     d = b * b - h;
00232
00233     if(d < 0.f)
00234         return false;
00235
00236     d  = osgSqrt(d);
00237     t1 = b - d;
00238
00239 //    if (t1 > 1)
00240 //        return false;
00241
00242     t2 = b + d;
00243
00244     if( t1 < Eps )
00245     {
00246         if( t2 < Eps /*|| t2 > 1*/)
00247         {
00248             return false;
00249         }
00250     }
00251
00252     enter = t1;
00253     exit  = t2;
00254
00255     return true;
00256 }
00257
00261 bool Line::intersect(const CylinderVolume &cyl) const
00262 {
00263     Real ent;
00264     Real ex;
00265
00266     return this->intersect(cyl, ent, ex);
00267 }
00268
00273 bool Line::intersect(const CylinderVolume &cyl,
00274                            Real           &enter,
00275                            Real           &exit ) const
00276 {
00277     Real  radius = cyl.getRadius();
00278
00279     Vec3r adir;
00280     Vec3r o_adir;
00281     Pnt3r apos;
00282
00283     cyl.getAxis(apos, adir);
00284
00285     o_adir = adir;
00286     adir.normalize();
00287
00288     bool isect;
00289
00290     Real  ln;
00291     Real  dl;
00292     Vec3r RC;
00293     Vec3r n;
00294     Vec3r D;
00295
00296     RC = _pos - apos;
00297
00298     n  = _dir.cross (adir);
00299     ln =  n  .length(    );
00300
00301     if(ln == 0.f)    // IntersectionLine is parallel to CylinderAxis
00302     {
00303         D  = RC - (RC.dot(adir)) * adir;
00304         dl = D.length();
00305
00306         if(dl <= radius)   // line lies in cylinder
00307         {
00308             enter = 0.f;
00309             exit  = Inf;
00310         }
00311         else
00312         {
00313             return false;
00314         }
00315     }
00316     else
00317     {
00318         n.normalize();
00319
00320         dl    = osgAbs(RC.dot(n));        //shortest distance
00321         isect = (dl <= radius);
00322
00323         if(isect)
00324         {                 // if ray hits cylinder
00325             Real  t;
00326             Real  s;
00327             Vec3r O;
00328
00329             O = RC.cross(adir);
00330             t = - (O.dot(n)) / ln;
00331             O = n.cross(adir);
00332
00333             O.normalize();
00334
00335             s = osgAbs (
00336                 (osgSqrt ((radius * radius) - (dl * dl))) / (_dir.dot(O)));
00337
00338             exit = t + s;
00339
00340             if(exit < 0.f)
00341                 return false;
00342
00343             enter = t - s;
00344
00345             if(enter < 0.f)
00346                 enter = 0.f;
00347         }
00348         else
00349         {
00350             return false;
00351         }
00352     }
00353
00354     Real t;
00355
00356     Plane bottom(-adir, apos);
00357
00358     if(bottom.intersect(*this, t))
00359     {
00360         if(bottom.isInHalfSpace(_pos))
00361         {
00362             if(t > enter)
00363                 enter = t;
00364         }
00365         else
00366         {
00367             if(t < exit)
00368                 exit = t;
00369         }
00370     }
00371     else
00372     {
00373         if(bottom.isInHalfSpace(_pos))
00374             return false;
00375     }
00376
00377     Plane top(adir, apos + o_adir);
00378
00379     if(top.intersect(*this, t))
00380     {
00381         if(top.isInHalfSpace(_pos))
00382         {
00383             if(t > enter)
00384                 enter = t;
00385         }
00386         else
00387         {
00388             if(t < exit)
00389                 exit = t;
00390         }
00391     }
00392     else
00393     {
00394         if(top.isInHalfSpace(_pos))
00395             return false;
00396     }
00397
00398     return (enter < exit);
00399 }
00400
00404 bool Line::intersect(const FrustumVolume &frustum) const
00405 {
00406     Real ent;
00407     Real ex;
00408
00409     return this->intersect(frustum, ent, ex);
00410 }
00411
00417 #if !defined(OSG_DO_DOC) || defined(OSG_DOC_DEV)
00418 
00421 struct face
00422 {
00423     Pnt3r point;
00424     Vec3r inner_vector;
00425     Vec3r inner_normal;
00426 };
00427
00428 #endif
00429 
00430 bool Line::intersect(const FrustumVolume &frustum ,
00431                            Real          &enter   ,
00432                            Real          &exit    ) const
00433 {
00434     const Real inf = 2u << 16;
00435
00436     Pnt3r enter_point = _pos + _dir * 0.f;
00437     Pnt3r exit_point  = _pos + _dir * inf;
00438
00439     face faces[6];
00440
00441     const Plane *planes = frustum.getPlanes();
00442
00443     Line lines[2];
00444
00445     //ln[0] - intersection of right and top planes
00446     planes[3].intersect(planes[4], lines[0]);
00447
00448     //ln[1] - left and bottom
00449     planes[2].intersect(planes[5], lines[1]);
00450
00451     Pnt3r pointA;
00452     Pnt3r pointB;
00453
00454     if(!planes[0].intersectInfinite(lines[0],pointA))
00455         std::cout << "This should never happen (A)!!!!";
00456
00457     if(!planes[1].intersectInfinite(lines[1],pointB))
00458         std::cout << "This should never happen (B)!!!!";
00459
00460     faces[0].point        = pointA;
00461     faces[0].inner_vector = pointB - pointA;
00462
00463     faces[1].point        = pointB;
00464     faces[1].inner_vector = pointA - pointB;
00465
00466     faces[2].point        = pointB;
00467     faces[2].inner_vector = pointA - pointB;
00468
00469     faces[3].point        = pointA;
00470     faces[3].inner_vector = pointB - pointA;
00471
00472     faces[4].point        = pointA;
00473     faces[4].inner_vector = pointB - pointA;
00474
00475     faces[5].point        = pointB;
00476     faces[5].inner_vector = pointA - pointB;
00477
00478     for(Int32 i = 0; i < 6; i++)
00479     {
00480         faces[i].inner_normal=planes[i].getNormal();
00481
00482         if(faces[i].inner_normal.dot(faces[i].inner_vector) < 0.f)
00483             faces[i].inner_normal=-faces[i].inner_normal;
00484
00485         Vec3r test_enp = enter_point - faces[i].point;
00486         Vec3r test_exp = exit_point  - faces[i].point;
00487
00488         Real value_enp = test_enp.dot(faces[i].inner_normal);
00489         Real value_exp = test_exp.dot(faces[i].inner_normal);
00490
00491         if(value_enp < 0.f && value_exp < 0.f)
00492             return false;
00493
00494         if(value_enp > 0.f && value_exp < 0.f)
00495             planes[i].intersect(*this, exit_point );
00496
00497         if(value_enp < 0.f && value_exp > 0.f)
00498             planes[i].intersect(*this, enter_point);
00499     }
00500
00501     Real a;
00502
00503     if((a = (enter_point - _pos).dot(_dir)) != 0.f)
00504     {
00505         enter = (enter_point - _pos).dot(enter_point - _pos) / a;
00506     }
00507     else
00508     {
00509         enter = 0.f;
00510     }
00511
00512     if((a = (exit_point  - _pos).dot(_dir)) != 0.f)
00513     {
00514         exit  = (exit_point  - _pos).dot(exit_point  - _pos) / a;
00515     }
00516     else
00517     {
00518         enter = 0.f;
00519     }
00520
00521     return true;
00522 }
00523
00524
00528 bool Line::intersect(const BoxVolume &box,
00529                            Real      &enter,
00530                            Real      &exit ) const
00531 {
00532     Pnt3r low;
00533     Pnt3r high;
00534
00535     box.getBounds(low, high);
00536
00537     Real r;
00538     Real te;
00539     Real tl;
00540
00541     Real in  = 0.f;
00542     Real out = Inf;
00543
00544     if(_dir[0] > Eps)
00545     {
00546         r = 1.f / _dir[0];
00547
00548         te = (low [0] - _pos[0]) * r;
00549         tl = (high[0] - _pos[0]) * r;
00550
00551         if(tl < out)
00552             out = tl;
00553
00554         if(te > in)
00555             in  = te;
00556     }
00557     else if(_dir[0] < -Eps)
00558     {
00559         r = 1.f / _dir[0];
00560
00561         te = (high[0] - _pos[0]) * r;
00562         tl = (low [0] - _pos[0]) * r;
00563
00564         if(tl < out)
00565             out = tl;
00566
00567         if(te > in)
00568             in  = te;
00569     }
00570     else if(_pos[0] < low[0] || _pos[0] > high[0])
00571     {
00572         return false;
00573     }
00574
00575     if(_dir[1] > Eps)
00576     {
00577         r = 1.f / _dir[1];
00578
00579         te = (low [1] - _pos[1]) * r;
00580         tl = (high[1] - _pos[1]) * r;
00581
00582         if(tl < out)
00583             out = tl;
00584
00585         if(te > in)
00586             in  = te;
00587
00588         if(in-out >= Eps)
00589             return false;
00590     }
00591     else if(_dir[1] < -Eps)
00592     {
00593         r = 1.f / _dir[1];
00594
00595         te = (high[1] - _pos[1]) * r;
00596         tl = (low [1] - _pos[1]) * r;
00597
00598         if(tl < out)
00599             out = tl;
00600
00601         if(te > in)
00602             in  = te;
00603
00604         if(in-out >= Eps)
00605             return false;
00606     }
00607     else if(_pos[1] < low[1] || _pos[1] > high[1])
00608     {
00609         return false;
00610     }
00611
00612     if(_dir[2] > Eps)
00613     {
00614         r = 1.f / _dir[2];
00615
00616         te = (low [2] - _pos[2]) * r;
00617         tl = (high[2] - _pos[2]) * r;
00618
00619         if(tl < out)
00620             out = tl;
00621
00622         if(te > in)
00623             in  = te;
00624     }
00625     else if(_dir[2] < -Eps)
00626     {
00627         r = 1.f / _dir[2];
00628
00629         te = (high[2] - _pos[2]) * r;
00630         tl = (low [2] - _pos[2]) * r;
00631
00632         if(tl < out)
00633             out = tl;
00634
00635         if(te > in)
00636             in  = te;
00637     }
00638     else if(_pos[2] < low[2] || _pos[2] > high[2])
00639     {
00640         return false;
00641     }
00642
00643     enter = in;
00644     exit  = out;
00645
00646     // Eps: count flat boxes as intersected
00647
00648
00649     // BUGFIXED, was:  
00650
00651     // return enter-exit < Eps;
00652
00653     // This got unstable with bbox check of huge planes
00654     // and if compiled as opt lib (at least with my gcc 4.0.2).
00655     // However it worked when compiled as dbg lib. 
00656
00657     // And now something completely different...
00658
00659     return in-out < Eps;
00660
00661     // To get you even more confused: It also works if you leave it
00662     // as "enter-exit" but declare in/out as Real64.
00663
00664     // Now think about it......and if it is not 42, please tell!
00665 }
00666
00667 #ifdef __sgi
00668 #pragma set woff 1209
00669 #endif
00670 
00674 bool Line::intersect(      Real       OSG_CHECK_ARG(angle),
00675                      const BoxVolume &OSG_CHECK_ARG(box  )) const
00676 {
00677     // TODO
00678     assert(false);
00679     return false;
00680 }
00681
00685 bool Line::intersect(      Real    OSG_CHECK_ARG(angle),
00686                      const Vec3r  &OSG_CHECK_ARG(point)) const
00687 {
00688     // TODO
00689     assert(false);
00690     return false;
00691 }
00692
00696 bool Line::intersect(      Real   OSG_CHECK_ARG(angle),
00697                      const Vec3r &OSG_CHECK_ARG(v0   ),
00698                      const Vec3r &OSG_CHECK_ARG(v1   ),
00699                            Vec3r &OSG_CHECK_ARG(pt   )) const
00700 {
00701     // TODO
00702     assert(false);
00703     return false;
00704 }
00705
00706 #ifdef __sgi
00707 #pragma reset woff 1209
00708 #endif
00709 
00721 bool Line::intersect(const Pnt3r &v0,
00722                      const Pnt3r &v1,
00723                      const Pnt3r &v2,
00724                            Real  &t,
00725                            Vec3r *norm) const
00726 {
00727     // Eps (1E-6f) didn't work with very small geometries!
00728     static const Real sEps = 1E-10f;
00729
00730     // find vectors for two edges sharing v0.
00731     Vec3r edge1 = v1 - v0;
00732     Vec3r edge2 = v2 - v0;
00733
00734     // begin calculating determinant - also used to calculate U parameter.
00735     Vec3r pvec = _dir.cross(edge2);
00736
00737     // if determinant is near zero, ray lies in plane of triangle.
00738     Real det = edge1.dot(pvec);
00739
00740     Vec3r qvec;
00741
00742     if(det > sEps)
00743     {
00744         // calculate distance from v0 to ray origin.
00745         Vec3r tvec = _pos - v0;
00746
00747         // calculate U parameter and test bounds.
00748         Real u = tvec.dot(pvec);
00749
00750         if(u < 0.f || u > det)
00751         {
00752             return false;
00753         }
00754
00755         // prepare to test V parameter.
00756         qvec = tvec.cross(edge1);
00757
00758         // calculate V parameter and test bounds.
00759         Real v = _dir.dot(qvec);
00760
00761         if(v < 0.f || u + v > det)
00762         {
00763             return false;
00764         }
00765     }
00766     else if(det < -sEps)
00767     {
00768         // calculate distance from v0 to ray origin.
00769         Vec3r tvec = _pos - v0;
00770
00771         // calculate U parameter and test bounds.
00772         Real u = tvec.dot(pvec);
00773
00774         if(u > 0.f || u < det)
00775         {
00776             return false;
00777         }
00778
00779         // prepare to test V parameter.
00780         qvec = tvec.cross(edge1);
00781
00782         // calculate V parameter and test bounds.
00783         Real v = _dir.dot(qvec);
00784
00785         if(v > 0.f || u + v < det)
00786         {
00787             return false;
00788         }
00789     }
00790     else
00791     {
00792         return false;  // ray is parallel to the plane of the triangle.
00793     }
00794
00795     Real inv_det = 1.0f / det;
00796
00797     // calculate t, ray intersects triangle.
00798     t = edge2.dot(qvec) * inv_det;
00799
00800     if(norm != NULL)
00801     {
00802         *norm = edge1.cross(edge2);
00803         norm->normalize();
00804     }
00805
00806     return true;
00807 }
00808
00809 /* Check this against Peter Shirleys code:
00810 
00811 float a = v1.x() - v2.x();
00812 float b = v1.y() - v2.y();
00813 float c = v1.z() - v2.z();
00814 
00815 float d = v1.x() - v3.x();
00816 float e = v1.y() - v3.y();
00817 float f = v1.z() - v3.z();
00818 
00819 float g = _d.x();
00820 float h = _d.y();
00821 float i = _d.z();
00822 
00823 float j = v1.x() - _s.x();
00824 float k = v1.y() - _s.y();
00825 float l = v1.z() - _s.z();
00826 
00827 float ei_hf = e*i-h*f;
00828 float gf_di = g*f-d*i;
00829 float dh_eg = d*h-e*g;
00830 float ak_jb = a*k-j*b;
00831 float jc_al = j*c-a*l;
00832 float bl_kc = b*l-k*c;
00833 
00834 float M = a*(ei_hf) + b*(gf_di) + c*(dh_eg);
00835 float beta = ( j*(ei_hf)+ k*(gf_di) + l*(dh_eg) ) / M;
00836 float gamma = ( i*(ak_jb) + h*(jc_al) + g*(bl_kc) ) / M;
00837 float t = -( f*(ak_jb) + e*(jc_al) + d*(bl_kc) ) / M;
00838 
00840 return (beta+gamma < 1.0f)
00841         && (beta > 0.0f) && (gamma > 0.0f)
00842         && (t >= 0.0f) && (t <= 1.0f);
00843 
00844 */
00845
00846 OSG_BASE_DLLMAPPING
00847 std::ostream &operator <<(std::ostream &outStream, const Line &obj)
00848 {
00849     return outStream << obj.getPosition() << ":" << obj.getDirection();
00850 }
00851
00852 OSG_END_NAMESPACE
00853
00854
00855
00856
00857