--- /dev/null
+///////////////////////////////////////////////////////////////////////////
+//
+// Copyright (c) 2002, Industrial Light & Magic, a division of Lucas
+// Digital Ltd. LLC
+//
+// All rights reserved.
+//
+// Redistribution and use in source and binary forms, with or without
+// modification, are permitted provided that the following conditions are
+// met:
+// * Redistributions of source code must retain the above copyright
+// notice, this list of conditions and the following disclaimer.
+// * Redistributions in binary form must reproduce the above
+// copyright notice, this list of conditions and the following disclaimer
+// in the documentation and/or other materials provided with the
+// distribution.
+// * Neither the name of Industrial Light & Magic nor the names of
+// its contributors may be used to endorse or promote products derived
+// from this software without specific prior written permission.
+//
+// THIS SOFTWARE IS PROVIDED BY THE COPYRIGHT HOLDERS AND CONTRIBUTORS
+// "AS IS" AND ANY EXPRESS OR IMPLIED WARRANTIES, INCLUDING, BUT NOT
+// LIMITED TO, THE IMPLIED WARRANTIES OF MERCHANTABILITY AND FITNESS FOR
+// A PARTICULAR PURPOSE ARE DISCLAIMED. IN NO EVENT SHALL THE COPYRIGHT
+// OWNER OR CONTRIBUTORS BE LIABLE FOR ANY DIRECT, INDIRECT, INCIDENTAL,
+// SPECIAL, EXEMPLARY, OR CONSEQUENTIAL DAMAGES (INCLUDING, BUT NOT
+// LIMITED TO, PROCUREMENT OF SUBSTITUTE GOODS OR SERVICES; LOSS OF USE,
+// DATA, OR PROFITS; OR BUSINESS INTERRUPTION) HOWEVER CAUSED AND ON ANY
+// THEORY OF LIABILITY, WHETHER IN CONTRACT, STRICT LIABILITY, OR TORT
+// (INCLUDING NEGLIGENCE OR OTHERWISE) ARISING IN ANY WAY OUT OF THE USE
+// OF THIS SOFTWARE, EVEN IF ADVISED OF THE POSSIBILITY OF SUCH DAMAGE.
+//
+///////////////////////////////////////////////////////////////////////////
+
+
+
+#ifndef INCLUDED_IMATHLINEALGO_H
+#define INCLUDED_IMATHLINEALGO_H
+
+//------------------------------------------------------------------
+//
+// This file contains algorithms applied to or in conjunction
+// with lines (Imath::Line). These algorithms may require
+// more headers to compile. The assumption made is that these
+// functions are called much less often than the basic line
+// functions or these functions require more support classes
+//
+// Contains:
+//
+// bool closestPoints(const Line<T>& line1,
+// const Line<T>& line2,
+// Vec3<T>& point1,
+// Vec3<T>& point2)
+//
+// bool intersect( const Line3<T> &line,
+// const Vec3<T> &v0,
+// const Vec3<T> &v1,
+// const Vec3<T> &v2,
+// Vec3<T> &pt,
+// Vec3<T> &barycentric,
+// bool &front)
+//
+// V3f
+// closestVertex(const Vec3<T> &v0,
+// const Vec3<T> &v1,
+// const Vec3<T> &v2,
+// const Line3<T> &l)
+//
+// V3f
+// nearestPointOnTriangle(const Vec3<T> &v0,
+// const Vec3<T> &v1,
+// const Vec3<T> &v2,
+// const Line3<T> &l)
+//
+// V3f
+// rotatePoint(const Vec3<T> p, Line3<T> l, float angle)
+//
+//------------------------------------------------------------------
+
+#include "ImathLine.h"
+#include "ImathVecAlgo.h"
+
+namespace Imath {
+
+
+template <class T>
+bool closestPoints(const Line3<T>& line1,
+ const Line3<T>& line2,
+ Vec3<T>& point1,
+ Vec3<T>& point2)
+{
+ //
+ // Compute the closest points on two lines. This was originally
+ // lifted from inventor. This function assumes that the line
+ // directions are normalized. The original math has been collapsed.
+ //
+
+ T A = line1.dir ^ line2.dir;
+
+ if ( A == 1 ) return false;
+
+ T denom = A * A - 1;
+
+ T B = (line1.dir ^ line1.pos) - (line1.dir ^ line2.pos);
+ T C = (line2.dir ^ line1.pos) - (line2.dir ^ line2.pos);
+
+ point1 = line1(( B - A * C ) / denom);
+ point2 = line2(( B * A - C ) / denom);
+
+ return true;
+}
+
+
+
+template <class T>
+bool intersect( const Line3<T> &line,
+ const Vec3<T> &v0,
+ const Vec3<T> &v1,
+ const Vec3<T> &v2,
+ Vec3<T> &pt,
+ Vec3<T> &barycentric,
+ bool &front)
+{
+ // Intersect the line with a triangle.
+ // 1. find plane of triangle
+ // 2. find intersection point of ray and plane
+ // 3. pick plane to project point and triangle into
+ // 4. check each edge of triangle to see if point is inside it
+
+ //
+ // XXX TODO - this routine is way too long
+ // - the value of EPSILON is dubious
+ // - there should be versions of this
+ // routine that do not calculate the
+ // barycentric coordinates or the
+ // front flag
+
+ const float EPSILON = 1e-6;
+
+ T d, t, d01, d12, d20, vd0, vd1, vd2, ax, ay, az, sense;
+ Vec3<T> v01, v12, v20, c;
+ int axis0, axis1;
+
+ // calculate plane for polygon
+ v01 = v1 - v0;
+ v12 = v2 - v1;
+
+ // c is un-normalized normal
+ c = v12.cross(v01);
+
+ d = c.length();
+ if(d < EPSILON)
+ return false; // cant hit a triangle with no area
+ c = c * (1. / d);
+
+ // calculate distance to plane along ray
+
+ d = line.dir.dot(c);
+ if (d < EPSILON && d > -EPSILON)
+ return false; // line is parallel to plane containing triangle
+
+ t = (v0 - line.pos).dot(c) / d;
+
+ if(t < 0)
+ return false;
+
+ // calculate intersection point
+ pt = line.pos + t * line.dir;
+
+ // is point inside triangle? Project to 2d to find out
+ // use the plane that has the largest absolute value
+ // component in the normal
+ ax = c[0] < 0 ? -c[0] : c[0];
+ ay = c[1] < 0 ? -c[1] : c[1];
+ az = c[2] < 0 ? -c[2] : c[2];
+
+ if(ax > ay && ax > az)
+ {
+ // project on x=0 plane
+
+ axis0 = 1;
+ axis1 = 2;
+ sense = c[0] < 0 ? -1 : 1;
+ }
+ else if(ay > az)
+ {
+ axis0 = 2;
+ axis1 = 0;
+ sense = c[1] < 0 ? -1 : 1;
+ }
+ else
+ {
+ axis0 = 0;
+ axis1 = 1;
+ sense = c[2] < 0 ? -1 : 1;
+ }
+
+ // distance from v0-v1 must be less than distance from v2 to v0-v1
+ d01 = sense * ((pt[axis0] - v0[axis0]) * v01[axis1]
+ - (pt[axis1] - v0[axis1]) * v01[axis0]);
+
+ if(d01 < 0) return false;
+
+ vd2 = sense * ((v2[axis0] - v0[axis0]) * v01[axis1]
+ - (v2[axis1] - v0[axis1]) * v01[axis0]);
+
+ if(d01 > vd2) return false;
+
+ // distance from v1-v2 must be less than distance from v1 to v2-v0
+ d12 = sense * ((pt[axis0] - v1[axis0]) * v12[axis1]
+ - (pt[axis1] - v1[axis1]) * v12[axis0]);
+
+ if(d12 < 0) return false;
+
+ vd0 = sense * ((v0[axis0] - v1[axis0]) * v12[axis1]
+ - (v0[axis1] - v1[axis1]) * v12[axis0]);
+
+ if(d12 > vd0) return false;
+
+ // calculate v20, and do check on final side of triangle
+ v20 = v0 - v2;
+ d20 = sense * ((pt[axis0] - v2[axis0]) * v20[axis1]
+ - (pt[axis1] - v2[axis1]) * v20[axis0]);
+
+ if(d20 < 0) return false;
+
+ vd1 = sense * ((v1[axis0] - v2[axis0]) * v20[axis1]
+ - (v1[axis1] - v2[axis1]) * v20[axis0]);
+
+ if(d20 > vd1) return false;
+
+ // vd0, vd1, and vd2 will always be non-zero for a triangle
+ // that has non-zero area (we return before this for
+ // zero area triangles)
+ barycentric = Vec3<T>(d12 / vd0, d20 / vd1, d01 / vd2);
+ front = line.dir.dot(c) < 0;
+
+ return true;
+}
+
+template <class T>
+Vec3<T>
+closestVertex(const Vec3<T> &v0,
+ const Vec3<T> &v1,
+ const Vec3<T> &v2,
+ const Line3<T> &l)
+{
+ Vec3<T> nearest = v0;
+ T neardot = (v0 - l.closestPointTo(v0)).length2();
+
+ T tmp = (v1 - l.closestPointTo(v1)).length2();
+
+ if (tmp < neardot)
+ {
+ neardot = tmp;
+ nearest = v1;
+ }
+
+ tmp = (v2 - l.closestPointTo(v2)).length2();
+ if (tmp < neardot)
+ {
+ neardot = tmp;
+ nearest = v2;
+ }
+
+ return nearest;
+}
+
+template <class T>
+Vec3<T>
+nearestPointOnTriangle(const Vec3<T> &v0,
+ const Vec3<T> &v1,
+ const Vec3<T> &v2,
+ const Line3<T> &l)
+{
+ Vec3<T> pt, barycentric;
+ bool front;
+
+ if (intersect (l, v0, v1, v2, pt, barycentric, front))
+ return pt;
+
+ //
+ // The line did not intersect the triangle, so to be picky, you should
+ // find the closest edge that it passed over/under, but chances are that
+ // 1) another triangle will be closer
+ // 2) the app does not need this much precision for a ray that does not
+ // intersect the triangle
+ // 3) the expense of the calculation is not worth it since this is the
+ // common case
+ //
+ // XXX TODO This is bogus -- nearestPointOnTriangle() should do
+ // what its name implies; it should return a point
+ // on an edge if some edge is closer to the line than
+ // any vertex. If the application does not want the
+ // extra calculations, it should be possible to specify
+ // that; it is not up to this nearestPointOnTriangle()
+ // to make the decision.
+
+ return closestVertex(v0, v1, v2, l);
+}
+
+template <class T>
+Vec3<T>
+rotatePoint(const Vec3<T> p, Line3<T> l, T angle)
+{
+ //
+ // Rotate the point p around the line l by the given angle.
+ //
+
+ //
+ // Form a coordinate frame with <x,y,a>. The rotation is the in xy
+ // plane.
+ //
+
+ Vec3<T> q = l.closestPointTo(p);
+ Vec3<T> x = p - q;
+ T radius = x.length();
+
+ x.normalize();
+ Vec3<T> y = (x % l.dir).normalize();
+
+ T cosangle = Math<T>::cos(angle);
+ T sinangle = Math<T>::sin(angle);
+
+ Vec3<T> r = q + x * radius * cosangle + y * radius * sinangle;
+
+ return r;
+}
+
+
+} // namespace Imath
+
+#endif
projects / opencv / blobdiff