mirror of https://github.com/opencv/opencv.git
Open Source Computer Vision Library
https://opencv.org/
You can not select more than 25 topics
Topics must start with a letter or number, can include dashes ('-') and can be up to 35 characters long.
287 lines
7.6 KiB
287 lines
7.6 KiB
/////////////////////////////////////////////////////////////////////////// |
|
// |
|
// 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 |
|
// rotatePoint(const Vec3<T> p, Line3<T> l, float angle) |
|
// |
|
//------------------------------------------------------------------ |
|
|
|
#include "ImathLine.h" |
|
#include "ImathVecAlgo.h" |
|
#include "ImathFun.h" |
|
|
|
namespace Imath { |
|
|
|
|
|
template <class T> |
|
bool |
|
closestPoints |
|
(const Line3<T>& line1, |
|
const Line3<T>& line2, |
|
Vec3<T>& point1, |
|
Vec3<T>& point2) |
|
{ |
|
// |
|
// Compute point1 and point2 such that point1 is on line1, point2 |
|
// is on line2 and the distance between point1 and point2 is minimal. |
|
// This function returns true if point1 and point2 can be computed, |
|
// or false if line1 and line2 are parallel or nearly parallel. |
|
// This function assumes that line1.dir and line2.dir are normalized. |
|
// |
|
|
|
Vec3<T> w = line1.pos - line2.pos; |
|
T d1w = line1.dir ^ w; |
|
T d2w = line2.dir ^ w; |
|
T d1d2 = line1.dir ^ line2.dir; |
|
T n1 = d1d2 * d2w - d1w; |
|
T n2 = d2w - d1d2 * d1w; |
|
T d = 1 - d1d2 * d1d2; |
|
T absD = abs (d); |
|
|
|
if ((absD > 1) || |
|
(abs (n1) < limits<T>::max() * absD && |
|
abs (n2) < limits<T>::max() * absD)) |
|
{ |
|
point1 = line1 (n1 / d); |
|
point2 = line2 (n2 / d); |
|
return true; |
|
} |
|
else |
|
{ |
|
return false; |
|
} |
|
} |
|
|
|
|
|
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) |
|
{ |
|
// |
|
// Given a line and a triangle (v0, v1, v2), the intersect() function |
|
// finds the intersection of the line and the plane that contains the |
|
// triangle. |
|
// |
|
// If the intersection point cannot be computed, either because the |
|
// line and the triangle's plane are nearly parallel or because the |
|
// triangle's area is very small, intersect() returns false. |
|
// |
|
// If the intersection point is outside the triangle, intersect |
|
// returns false. |
|
// |
|
// If the intersection point, pt, is inside the triangle, intersect() |
|
// computes a front-facing flag and the barycentric coordinates of |
|
// the intersection point, and returns true. |
|
// |
|
// The front-facing flag is true if the dot product of the triangle's |
|
// normal, (v2-v1)%(v1-v0), and the line's direction is negative. |
|
// |
|
// The barycentric coordinates have the following property: |
|
// |
|
// pt = v0 * barycentric.x + v1 * barycentric.y + v2 * barycentric.z |
|
// |
|
|
|
Vec3<T> edge0 = v1 - v0; |
|
Vec3<T> edge1 = v2 - v1; |
|
Vec3<T> normal = edge1 % edge0; |
|
|
|
T l = normal.length(); |
|
|
|
if (l != 0) |
|
normal /= l; |
|
else |
|
return false; // zero-area triangle |
|
|
|
// |
|
// d is the distance of line.pos from the plane that contains the triangle. |
|
// The intersection point is at line.pos + (d/nd) * line.dir. |
|
// |
|
|
|
T d = normal ^ (v0 - line.pos); |
|
T nd = normal ^ line.dir; |
|
|
|
if (abs (nd) > 1 || abs (d) < limits<T>::max() * abs (nd)) |
|
pt = line (d / nd); |
|
else |
|
return false; // line and plane are nearly parallel |
|
|
|
// |
|
// Compute the barycentric coordinates of the intersection point. |
|
// The intersection is inside the triangle if all three barycentric |
|
// coordinates are between zero and one. |
|
// |
|
|
|
{ |
|
Vec3<T> en = edge0.normalized(); |
|
Vec3<T> a = pt - v0; |
|
Vec3<T> b = v2 - v0; |
|
Vec3<T> c = (a - en * (en ^ a)); |
|
Vec3<T> d = (b - en * (en ^ b)); |
|
T e = c ^ d; |
|
T f = d ^ d; |
|
|
|
if (e >= 0 && e <= f) |
|
barycentric.z = e / f; |
|
else |
|
return false; // outside |
|
} |
|
|
|
{ |
|
Vec3<T> en = edge1.normalized(); |
|
Vec3<T> a = pt - v1; |
|
Vec3<T> b = v0 - v1; |
|
Vec3<T> c = (a - en * (en ^ a)); |
|
Vec3<T> d = (b - en * (en ^ b)); |
|
T e = c ^ d; |
|
T f = d ^ d; |
|
|
|
if (e >= 0 && e <= f) |
|
barycentric.x = e / f; |
|
else |
|
return false; // outside |
|
} |
|
|
|
barycentric.y = 1 - barycentric.x - barycentric.z; |
|
|
|
if (barycentric.y < 0) |
|
return false; // outside |
|
|
|
front = ((line.dir ^ normal) < 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> |
|
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
|
|
|