kindred/create
1
1
Fork 0
Code Issues 44 Pull Requests Actions Packages Projects 9 Releases 2 Wiki Activity
Files
0403ba68fa50705bc7df18d8eae32065ed3a2354
create/src/Mod/Mesh/App/Core/Elements.cpp

1398 lines
42 KiB
C++
Raw Blame History

/***************************************************************************
* Copyright (c) 2005 Imetric 3D GmbH *
* *
* This file is part of the FreeCAD CAx development system. *
* *
* This library is free software; you can redistribute it and/or *
* modify it under the terms of the GNU Library General Public *
* License as published by the Free Software Foundation; either *
* version 2 of the License, or (at your option) any later version. *
* *
* This library is distributed in the hope that it will be useful, *
* but WITHOUT ANY WARRANTY; without even the implied warranty of *
* MERCHANTABILITY or FITNESS FOR A PARTICULAR PURPOSE. See the *
* GNU Library General Public License for more details. *
* *
* You should have received a copy of the GNU Library General Public *
* License along with this library; see the file COPYING.LIB. If not, *
* write to the Free Software Foundation, Inc., 59 Temple Place, *
* Suite 330, Boston, MA 02111-1307, USA *
* *
***************************************************************************/
#include "PreCompiled.h"
#ifndef _PreComp_
#endif
#include <Mod/Mesh/App/WildMagic4/Wm4IntrSegment3Plane3.h>
#include <Mod/Mesh/App/WildMagic4/Wm4IntrSegment3Box3.h>
#include <Mod/Mesh/App/WildMagic4/Wm4DistVector3Triangle3.h>
#include <Mod/Mesh/App/WildMagic4/Wm4DistSegment3Triangle3.h>
#include "Elements.h"
#include "Algorithm.h"
#include "tritritest.h"
#include "Utilities.h"
using namespace MeshCore;
using namespace Wm4;
unsigned long MeshPointArray::Get (const MeshPoint &rclPoint)
{
iterator clIter;
clIter = std::find(begin(), end(), rclPoint);
if (clIter != end())
return clIter - begin();
else
return ULONG_MAX;
}
unsigned long MeshPointArray::GetOrAddIndex (const MeshPoint &rclPoint)
{
unsigned long ulIndex;
if ((ulIndex = Get(rclPoint)) == ULONG_MAX)
{
push_back(rclPoint);
return static_cast<unsigned long>(size() - 1);
}
else
return ulIndex;
}
void MeshPointArray::SetFlag (MeshPoint::TFlagType tF) const
{
for (MeshPointArray::_TConstIterator i = begin(); i < end(); ++i) i->SetFlag(tF);
}
void MeshPointArray::ResetFlag (MeshPoint::TFlagType tF) const
{
for (MeshPointArray::_TConstIterator i = begin(); i < end(); ++i) i->ResetFlag(tF);
}
void MeshPointArray::SetProperty (unsigned long ulVal) const
{
for (_TConstIterator pP = begin(); pP != end(); ++pP) pP->SetProperty(ulVal);
}
void MeshPointArray::ResetInvalid (void) const
{
for (_TConstIterator pP = begin(); pP != end(); ++pP) pP->ResetInvalid();
}
MeshPointArray& MeshPointArray::operator = (const MeshPointArray &rclPAry)
{
// std::vector<MeshPoint>::operator=(rclPAry);
TMeshPointArray::operator=(rclPAry);
return *this;
}
void MeshPointArray::Transform(const Base::Matrix4D& mat)
{
for (_TIterator pP = begin(); pP != end(); ++pP)
mat.multVec(*pP,*pP);
}
void MeshFacetArray::Erase (_TIterator pIter)
{
unsigned long i, *pulN;
_TIterator pPass, pEnd;
unsigned long ulInd = pIter - begin();
erase(pIter);
pPass = begin();
pEnd = end();
while (pPass < pEnd)
{
for (i = 0; i < 3; i++)
{
pulN = &pPass->_aulNeighbours[i];
if ((*pulN > ulInd) && (*pulN != ULONG_MAX))
(*pulN)--;
}
pPass++;
}
}
void MeshFacetArray::TransposeIndices (unsigned long ulOrig, unsigned long ulNew)
{
_TIterator pIter = begin(), pEnd = end();
while (pIter < pEnd)
{
pIter->Transpose(ulOrig, ulNew);
++pIter;
}
}
void MeshFacetArray::DecrementIndices (unsigned long ulIndex)
{
_TIterator pIter = begin(), pEnd = end();
while (pIter < pEnd)
{
pIter->Decrement(ulIndex);
++pIter;
}
}
void MeshFacetArray::SetFlag (MeshFacet::TFlagType tF) const
{
for (MeshFacetArray::_TConstIterator i = begin(); i < end(); ++i) i->SetFlag(tF);
}
void MeshFacetArray::ResetFlag (MeshFacet::TFlagType tF) const
{
for (MeshFacetArray::_TConstIterator i = begin(); i < end(); ++i) i->ResetFlag(tF);
}
void MeshFacetArray::SetProperty (unsigned long ulVal) const
{
for (_TConstIterator pF = begin(); pF != end(); ++pF) pF->SetProperty(ulVal);
}
void MeshFacetArray::ResetInvalid (void) const
{
for (_TConstIterator pF = begin(); pF != end(); ++pF) pF->ResetInvalid();
}
MeshFacetArray& MeshFacetArray::operator = (const MeshFacetArray &rclFAry)
{
TMeshFacetArray::operator=(rclFAry);
return *this;
}
// -----------------------------------------------------------------
bool MeshGeomEdge::ContainedByOrIntersectBoundingBox ( const Base::BoundBox3f &rclBB ) const
{
// Test, ob alle Eckpunkte der Edge sich auf einer der 6 Seiten der BB befinden
if ((GetBoundBox() && rclBB) == false)
return false;
// Test, ob Edge-BB komplett in BB liegt
if (rclBB.IsInBox(GetBoundBox()))
return true;
// Test, ob einer der Eckpunkte in BB liegt
for (int i=0;i<2;i++)
{
if (rclBB.IsInBox(_aclPoints[i]))
return true;
}
// "echter" Test auf Schnitt
if (IntersectBoundingBox(rclBB))
return true;
return false;
}
Base::BoundBox3f MeshGeomEdge::GetBoundBox () const
{
return Base::BoundBox3f(_aclPoints,2);
}
bool MeshGeomEdge::IntersectBoundingBox (const Base::BoundBox3f &rclBB) const
{
const Base::Vector3f& rclP0 = _aclPoints[0];
const Base::Vector3f& rclP1 = _aclPoints[1];
Vector3<float> A(rclP0.x, rclP0.y, rclP0.z);
Vector3<float> B(rclP1.x, rclP1.y, rclP1.z);
Vector3<float> n = B - A;
float len = n.Length();
n.Normalize();
Vector3<float> p = 0.5f*(A + B);
Segment3<float> akSeg(p, n, 0.5f*len);
Base::Vector3f clCenter = rclBB.GetCenter();
Vector3<float> center(clCenter.x, clCenter.y, clCenter.z);
Vector3<float> axis0(1.0f, 0.0f, 0.0f);
Vector3<float> axis1(0.0f, 1.0f, 0.0f);
Vector3<float> axis2(0.0f, 0.0f, 1.0f);
float extent0 = 0.5f*rclBB.LengthX();
float extent1 = 0.5f*rclBB.LengthY();
float extent2 = 0.5f*rclBB.LengthZ();
Box3<float> kBox(center, axis0, axis1, axis2, extent0, extent1, extent2);
IntrSegment3Box3<float> intrsectbox(akSeg, kBox, false);
return intrsectbox.Test();
}
bool MeshGeomEdge::IntersectWithLine (const Base::Vector3f &rclPt,
const Base::Vector3f &rclDir,
Base::Vector3f &rclRes) const
{
const float eps = 1e-06f;
Base::Vector3f n = _aclPoints[1] - _aclPoints[0];
// check angle between edge and the line direction, FLOAT_MAX is
// returned for degenerated edges
float fAngle = rclDir.GetAngle(n);
if (fAngle == 0) {
// parallel lines
float distance = _aclPoints[0].DistanceToLine(rclPt, rclDir);
if (distance < eps) {
// lines are equal
rclRes = _aclPoints[0];
return true;
}
return false; // no intersection possible
}
// that's the normal of a helper plane and its base at _aclPoints
Base::Vector3f normal = n.Cross(rclDir);
// if the distance of rclPt to the plane is higher than eps then the
// two lines are warped and there is no intersection possible
if (fabs(rclPt.DistanceToPlane(_aclPoints[0], normal)) > eps)
return false;
// get a second helper plane and get the intersection with the line
Base::Vector3f normal2 = normal.Cross(n);
float s = ((_aclPoints[0] - rclPt) * normal2) / (rclDir * normal2);
rclRes = rclPt + s * rclDir;
float dist1 = Base::Distance(_aclPoints[0], _aclPoints[1]);
float dist2 = Base::Distance(_aclPoints[0], rclRes);
float dist3 = Base::Distance(_aclPoints[1], rclRes);
return dist2 + dist3 <= dist1 + eps;
}
bool MeshGeomEdge::IntersectWithPlane (const Base::Vector3f &rclPt,
const Base::Vector3f &rclDir,
Base::Vector3f &rclRes) const
{
float dist1 = _aclPoints[0].DistanceToPlane(rclPt, rclDir);
float dist2 = _aclPoints[1].DistanceToPlane(rclPt, rclDir);
// either both points are below or above the plane
if (dist1 * dist2 >= 0.0f)
return false;
Base::Vector3f u = _aclPoints[1] - _aclPoints[0];
Base::Vector3f b = rclPt - _aclPoints[0];
float t = b.Dot(rclDir) / u.Dot(rclDir);
rclRes = _aclPoints[0] + t * u;
return true;
}
void MeshGeomEdge::ProjectPointToLine (const Base::Vector3f &rclPoint,
Base::Vector3f &rclProj) const
{
Base::Vector3f pt1 = rclPoint - _aclPoints[0];
Base::Vector3f dir = _aclPoints[1] - _aclPoints[0];
Base::Vector3f vec;
vec.ProjectToLine(pt1, dir);
rclProj = rclPoint + vec;
}
void MeshGeomEdge::ClosestPointsToLine(const Base::Vector3f &linePt, const Base::Vector3f &lineDir,
Base::Vector3f& rclPnt1, Base::Vector3f& rclPnt2) const
{
const float eps = 1e-06f;
Base::Vector3f edgeDir = _aclPoints[1] - _aclPoints[0];
// check angle between edge and the line direction, FLOAT_MAX is
// returned for degenerated edges
float fAngle = lineDir.GetAngle(edgeDir);
if (fAngle == 0) {
// parallel lines
float distance = _aclPoints[0].DistanceToLine(linePt, lineDir);
if (distance < eps) {
// lines are equal
rclPnt1 = _aclPoints[0];
rclPnt2 = _aclPoints[0];
}
else {
rclPnt1 = _aclPoints[0];
MeshGeomEdge edge;
edge._aclPoints[0] = linePt;
edge._aclPoints[1] = linePt + lineDir;
edge.ProjectPointToLine(rclPnt1, rclPnt2);
}
}
else {
// that's the normal of a helper plane
Base::Vector3f normal = edgeDir.Cross(lineDir);
// get a second helper plane and get the intersection with the line
Base::Vector3f normal2 = normal.Cross(edgeDir);
float s = ((_aclPoints[0] - linePt) * normal2) / (lineDir * normal2);
rclPnt2 = linePt + s * lineDir;
// get a third helper plane and get the intersection with the line
Base::Vector3f normal3 = normal.Cross(lineDir);
float t = ((linePt - _aclPoints[0]) * normal3) / (edgeDir * normal3);
rclPnt1 = _aclPoints[0] + t * edgeDir;
}
}
// -----------------------------------------------------------------
MeshGeomFacet::MeshGeomFacet (void)
: _bNormalCalculated(false),
_ucFlag(0), _ulProp(0)
{
}
MeshGeomFacet::MeshGeomFacet (const Base::Vector3f &v1,const Base::Vector3f &v2,const Base::Vector3f &v3)
: _bNormalCalculated(false),
_ucFlag(0),
_ulProp(0)
{
_aclPoints[0] = v1;
_aclPoints[1] = v2;
_aclPoints[2] = v3;
}
bool MeshGeomFacet::IsPointOf (const Base::Vector3f &rclPoint, float fDistance) const
{
if (DistancePlaneToPoint(rclPoint) > fDistance)
return false;
// force internal normal to be computed if not done yet
Base::Vector3f clNorm(GetNormal()), clProjPt(rclPoint), clEdge;
Base::Vector3f clP0(_aclPoints[0]), clP1(_aclPoints[1]), clP2(_aclPoints[2]);
float fLP, fLE;
clNorm.Normalize();
clProjPt.ProjectToPlane(_aclPoints[0], clNorm);
// Kante P0 --> P1
clEdge = clP1 - clP0;
fLP = clProjPt.DistanceToLine(clP0, clEdge);
if (fLP > 0.0f)
{
fLE = clP2.DistanceToLine(clP0, clEdge);
if (fLP <= fLE)
{
if (clProjPt.DistanceToLine(clP2, clEdge) > fLE)
return false;
}
else
return false;
}
// Kante P0 --> P2
clEdge = clP2 - clP0;
fLP = clProjPt.DistanceToLine(clP0, clEdge);
if (fLP > 0.0f)
{
fLE = clP1.DistanceToLine(clP0, clEdge);
if (fLP <= fLE)
{
if (clProjPt.DistanceToLine(clP1, clEdge) > fLE)
return false;
}
else
return false;
}
// Kante P1 --> P2
clEdge = clP2 - clP1;
fLP = clProjPt.DistanceToLine(clP1, clEdge);
if (fLP > 0.0f)
{
fLE = clP0.DistanceToLine(clP1, clEdge);
if (fLP <= fLE)
{
if (clProjPt.DistanceToLine(clP0, clEdge) > fLE)
return false;
}
else
return false;
}
return true;
}
bool MeshGeomFacet::IsPointOfFace (const Base::Vector3f& rclP, float fDistance) const
{
// effektivere Implementierung als in MeshGeomFacet::IsPointOf
//
Base::Vector3f a(_aclPoints[0].x, _aclPoints[0].y, _aclPoints[0].z);
Base::Vector3f b(_aclPoints[1].x, _aclPoints[1].y, _aclPoints[1].z);
Base::Vector3f c(_aclPoints[2].x, _aclPoints[2].y, _aclPoints[2].z);
Base::Vector3f p(rclP);
Base::Vector3f n = (b - a) % (c - a);
Base::Vector3f n1 = (a - p) % (b - p);
Base::Vector3f n2 = (c - p) % (a - p);
Base::Vector3f n3 = (b - p) % (c - p);
if (n * (p - a) > fDistance * n.Length())
return false;
if (n * (a - p) > fDistance * n.Length())
return false;
if (n * n1 <= 0.0f)
return false;
if (n * n2 <= 0.0f)
return false;
if (n * n3 <= 0.0f)
return false;
return true;
}
bool MeshGeomFacet::Weights(const Base::Vector3f& rclP, float& w0, float& w1, float& w2) const
{
float fAreaABC = Area();
float fAreaPBC = MeshGeomFacet(rclP,_aclPoints[1],_aclPoints[2]).Area();
float fAreaPCA = MeshGeomFacet(rclP,_aclPoints[2],_aclPoints[0]).Area();
float fAreaPAB = MeshGeomFacet(rclP,_aclPoints[0],_aclPoints[1]).Area();
w0=fAreaPBC/fAreaABC;
w1=fAreaPCA/fAreaABC;
w2=fAreaPAB/fAreaABC;
return fabs(w0+w1+w2-1.0f)<0.001f;
}
void MeshGeomFacet::ProjectPointToPlane (const Base::Vector3f &rclPoint, Base::Vector3f &rclProj) const
{
rclPoint.ProjectToPlane(_aclPoints[0], GetNormal(), rclProj);
}
void MeshGeomFacet::ProjectFacetToPlane (MeshGeomFacet &rclFacet) const
{
// project facet 2 onto facet 1
IntersectPlaneWithLine( rclFacet._aclPoints[0], GetNormal(), rclFacet._aclPoints[0] );
IntersectPlaneWithLine( rclFacet._aclPoints[1], GetNormal(), rclFacet._aclPoints[1] );
IntersectPlaneWithLine( rclFacet._aclPoints[2], GetNormal(), rclFacet._aclPoints[2] );
}
void MeshGeomFacet::Enlarge (float fDist)
{
Base::Vector3f clM, clU, clV, clPNew[3];
float fA, fD;
unsigned long i, ulP1, ulP2, ulP3;
for (i = 0; i < 3; i++)
{
ulP1 = i;
ulP2 = (i + 1) % 3;
ulP3 = (i + 2) % 3;
clU = _aclPoints[ulP2] - _aclPoints[ulP1];
clV = _aclPoints[ulP3] - _aclPoints[ulP1];
clM = -(clU + clV);
fA = clM.GetAngle(-clU);
fD = fDist / float(sin(fA));
clM.Normalize();
clM.Scale(fD, fD, fD);
clPNew[ulP1] = _aclPoints[ulP1] + clM;
}
_aclPoints[0] = clPNew[0];
_aclPoints[1] = clPNew[1];
_aclPoints[2] = clPNew[2];
}
bool MeshGeomFacet::IsDegenerated(float epsilon) const
{
// The triangle has the points A,B,C where we can define the vector u and v
// u = b-a and v = c-a. Then we define the line g: r = a+t*u and the plane
// E: (x-c)*u=0. The intersection point of g and E is S.
// The vector to S can be computed with a+(uv)/(uu)*u. The difference of
// C and S then is v-(u*v)/(u*u)*u. The square distance leads to the formula
// (v-(u*v)/(u*u)*u)^2 < eps which means that C and S is considered equal if
// the square distance is less than an epsilon and thus the triangle is de-
// generated.
// After a few calculation step we get the formula:
// (u*u)*(v*v)-(u*v)*(u*v) < eps*(u*u)
// So, if we do the same except that we define a line h which goes through
// A and C and a plane going through B we get a similar formula:
// (u*u)*(v*v)-(u*v)*(u*v) < eps*(v*v).
// As end formula we can write then:
// (u*u)*(v*v)-(u*v)*(u*v) < max(eps*(u*u),eps*(v*v)).
//
// BTW (u*u)*(v*v)-(u*v)*(u*v) is the same as (uxv)*(uxv).
Base::Vector3d p1 = Base::convertTo<Base::Vector3d>(this->_aclPoints[0]);
Base::Vector3d p2 = Base::convertTo<Base::Vector3d>(this->_aclPoints[1]);
Base::Vector3d p3 = Base::convertTo<Base::Vector3d>(this->_aclPoints[2]);
Base::Vector3d u = p2 - p1;
Base::Vector3d v = p3 - p1;
double eps = static_cast<double>(epsilon);
double uu = u*u;
if (uu <= eps)
return true;
double vv = v*v;
if (vv <= eps)
return true;
double uv = u*v;
double crosssqr = uu*vv-uv*uv;
if (crosssqr <= eps*std::max<double>(uu,vv))
return true;
return false;
}
bool MeshGeomFacet::IsDeformed(float fCosOfMinAngle, float fCosOfMaxAngle) const
{
float fCosAngle;
Base::Vector3f u,v;
for (int i=0; i<3; i++) {
u = _aclPoints[(i+1)%3]-_aclPoints[i];
v = _aclPoints[(i+2)%3]-_aclPoints[i];
u.Normalize();
v.Normalize();
fCosAngle = u * v;
if (fCosAngle > fCosOfMinAngle || fCosAngle < fCosOfMaxAngle)
return true;
}
return false;
}
bool MeshGeomFacet::IntersectBoundingBox ( const Base::BoundBox3f &rclBB ) const
{
// the triangle's corner points
const Base::Vector3f& v0 = _aclPoints[0];
const Base::Vector3f& v1 = _aclPoints[1];
const Base::Vector3f& v2 = _aclPoints[2];
// first check if at least one point is inside the box
if ( rclBB.IsInBox( v0 ) || rclBB.IsInBox( v1 ) || rclBB.IsInBox( v2 ) )
return true;
// edge lengths
float len0 = (v0-v1).Length();
float len1 = (v1-v2).Length();
float len2 = (v2-v0).Length();
// Build up the line segments
Vector3<float> p0(0.5f*(v0.x+v1.x), 0.5f*(v0.y+v1.y), 0.5f*(v0.z+v1.z));
Vector3<float> p1(0.5f*(v1.x+v2.x), 0.5f*(v1.y+v2.y), 0.5f*(v1.z+v2.z));
Vector3<float> p2(0.5f*(v2.x+v0.x), 0.5f*(v2.y+v0.y), 0.5f*(v2.z+v0.z));
Vector3<float> d0(v1.x - v0.x, v1.y - v0.y, v1.z - v0.z);
d0.Normalize();
Vector3<float> d1(v2.x - v1.x, v2.y - v1.y, v2.z - v1.z);
d1.Normalize();
Vector3<float> d2(v0.x - v2.x, v0.y - v2.y, v0.z - v2.z);
d2.Normalize();
Segment3<float> akSeg0(p0, d0, len0/2.0f );
Segment3<float> akSeg1(p1, d1, len1/2.0f);
Segment3<float> akSeg2(p2, d2, len2/2.0f);
// Build up the box
Base::Vector3f clCenter = rclBB.GetCenter();
Vector3<float> center(clCenter.x, clCenter.y, clCenter.z);
Vector3<float> axis0(1.0f, 0.0f, 0.0f);
Vector3<float> axis1(0.0f, 1.0f, 0.0f);
Vector3<float> axis2(0.0f, 0.0f, 1.0f);
float extent0 = 0.5f*rclBB.LengthX();
float extent1 = 0.5f*rclBB.LengthY();
float extent2 = 0.5f*rclBB.LengthZ();
Box3<float> akBox(center, axis0, axis1, axis2, extent0, extent1, extent2);
// Check for intersection of line segments and box
IntrSegment3Box3<float> akSec0(akSeg0, akBox, false);
if ( akSec0.Test() )
return true;
IntrSegment3Box3<float> akSec1(akSeg1, akBox, false);
if ( akSec1.Test() )
return true;
IntrSegment3Box3<float> akSec2(akSeg2, akBox, false);
if ( akSec2.Test() )
return true;
// no intersection
return false;
}
bool MeshGeomFacet::IntersectWithPlane (const Base::Vector3f &rclBase, const Base::Vector3f &rclNormal, Base::Vector3f &rclP1, Base::Vector3f &rclP2) const
{
const float eps = 1e-06f;
// the triangle's corner points
const Base::Vector3f& v0 = _aclPoints[0];
const Base::Vector3f& v1 = _aclPoints[1];
const Base::Vector3f& v2 = _aclPoints[2];
// first check if a triangle's edge lies on the plane
float dist0 = fabs(v0.DistanceToPlane(rclBase, rclNormal));
float dist1 = fabs(v1.DistanceToPlane(rclBase, rclNormal));
float dist2 = fabs(v2.DistanceToPlane(rclBase, rclNormal));
if (dist0 < eps && dist1 < eps) {
rclP1 = v0;
rclP2 = v1;
return true;
}
if (dist1 < eps && dist2 < eps) {
rclP1 = v1;
rclP2 = v2;
return true;
}
if (dist2 < eps && dist0 < eps) {
rclP1 = v2;
rclP2 = v0;
return true;
}
// edge lengths
float len0 = (v0-v1).Length();
float len1 = (v1-v2).Length();
float len2 = (v2-v0).Length();
// Build up the line segments
Vector3<float> p0(0.5f*(v0.x+v1.x), 0.5f*(v0.y+v1.y), 0.5f*(v0.z+v1.z));
Vector3<float> p1(0.5f*(v1.x+v2.x), 0.5f*(v1.y+v2.y), 0.5f*(v1.z+v2.z));
Vector3<float> p2(0.5f*(v2.x+v0.x), 0.5f*(v2.y+v0.y), 0.5f*(v2.z+v0.z));
Vector3<float> d0(v1.x - v0.x, v1.y - v0.y, v1.z - v0.z);
d0.Normalize();
Vector3<float> d1(v2.x - v1.x, v2.y - v1.y, v2.z - v1.z);
d1.Normalize();
Vector3<float> d2(v0.x - v2.x, v0.y - v2.y, v0.z - v2.z);
d2.Normalize();
Segment3<float> akSeg0(p0, d0, len0/2.0f );
Segment3<float> akSeg1(p1, d1, len1/2.0f);
Segment3<float> akSeg2(p2, d2, len2/2.0f);
// Build up the plane
Vector3<float> p(rclBase.x, rclBase.y, rclBase.z);
Vector3<float> n(rclNormal.x, rclNormal.y, rclNormal.z);
Plane3<float> akPln(n, p);
// Check for intersection with plane for each line segment
IntrSegment3Plane3<float> test0(akSeg0, akPln);
IntrSegment3Plane3<float> test1(akSeg1, akPln);
IntrSegment3Plane3<float> test2(akSeg2, akPln);
Vector3<float> intr;
// now check if a triangle's corner lies on the plane
if (dist0 < eps) {
rclP1 = v0;
rclP2 = v0;
if (test1.Find()) {
intr = p1 + test1.GetSegmentT() * d1;
rclP2.Set(intr[0], intr[1], intr[2]);
}
return true;
}
else if (dist1 < eps) {
rclP1 = v1;
rclP2 = v1;
if (test2.Find()) {
intr = p2 + test2.GetSegmentT() * d2;
rclP2.Set(intr[0], intr[1], intr[2]);
}
return true;
}
else if (dist2 < eps) {
rclP1 = v2;
rclP2 = v2;
if (test0.Find()) {
intr = p0 + test0.GetSegmentT() * d0;
rclP2.Set(intr[0], intr[1], intr[2]);
}
return true;
}
// check for arbitrary intersections
if (test0.Find()) {
intr = p0 + test0.GetSegmentT() * d0;
rclP1.Set( intr[0], intr[1], intr[2]);
if (test1.Find()) {
intr = p1 + test1.GetSegmentT() * d1;
rclP2.Set( intr[0], intr[1], intr[2]);
return true;
}
else if (test2.Find()) {
intr = p2 + test2.GetSegmentT() * d2;
rclP2.Set( intr[0], intr[1], intr[2]);
return true;
}
}
else if (test1.Find()) {
intr = p1 + test1.GetSegmentT() * d1;
rclP1.Set( intr[0], intr[1], intr[2]);
if (test2.Find()) {
intr = p2 + test2.GetSegmentT() * d2;
rclP2.Set( intr[0], intr[1], intr[2]);
return true;
}
}
return false;
}
bool MeshGeomFacet::Foraminate (const Base::Vector3f &P, const Base::Vector3f &dir, Base::Vector3f &I, float fMaxAngle) const
{
const float eps = 1e-06f;
Base::Vector3f n = this->GetNormal();
// check angle between facet normal and the line direction, FLOAT_MAX is
// returned for degenerated facets
float fAngle = dir.GetAngle(n);
if (fAngle > fMaxAngle)
return false;
float nn = n * n;
float nd = n * dir;
float dd = dir * dir;
// the line mustn't be parallel to the triangle
if ((nd * nd) <= (eps * dd * nn))
return false;
Base::Vector3f u = this->_aclPoints[1] - this->_aclPoints[0];
Base::Vector3f v = this->_aclPoints[2] - this->_aclPoints[0];
Base::Vector3f w0 = P - this->_aclPoints[0];
float r = -(n * w0) / nd;
Base::Vector3f w = w0 + r * dir;
float uu = u * u;
float uv = u * v;
float vv = v * v;
float wu = w * u;
float wv = w * v;
float det = float(fabs((uu * vv) - (uv * uv)));
float s = (vv * wu) - (uv * wv);
float t = (uu * wv) - (uv * wu);
// is the intersection point inside the triangle?
if ((s >= 0.0f) && (t >= 0.0f) && ((s + t) <= det)) {
I = w + this->_aclPoints[0];
return true;
}
return false;
}
bool MeshGeomFacet::IntersectPlaneWithLine (const Base::Vector3f &rclPt, const Base::Vector3f &rclDir, Base::Vector3f &rclRes) const
{
// berechne den Schnittpunkt Gerade <-> Ebene
if ( fabs(rclDir * GetNormal()) < 1e-3f )
return false; // line and plane are parallel
float s = ( ( GetGravityPoint() - rclPt ) * GetNormal() )
/ ( rclDir * GetNormal() );
rclRes = rclPt + s * rclDir;
return true;
}
bool MeshGeomFacet::IntersectWithLine (const Base::Vector3f &rclPt, const Base::Vector3f &rclDir, Base::Vector3f &rclRes) const
{
if ( !IntersectPlaneWithLine( rclPt, rclDir, rclRes ) )
return false; // line and plane are parallel
// Check if the intersection point is inside the facet
return IsPointOfFace(rclRes, 1e-03f);
}
float MeshGeomFacet::DistanceToLineSegment (const Base::Vector3f &rclP1, const Base::Vector3f &rclP2) const
{
// line segment
Vector3<float> A(rclP1.x, rclP1.y, rclP1.z);
Vector3<float> B(rclP2.x, rclP2.y, rclP2.z);
Vector3<float> n = B - A;
float len = n.Length();
n.Normalize();
Vector3<float> p = 0.5f*(A + B);
Segment3<float> akSeg(p, n, 0.5f*len);
// triangle
Vector3<float> akF0(_aclPoints[0].x, _aclPoints[0].y, _aclPoints[0].z);
Vector3<float> akF1(_aclPoints[1].x, _aclPoints[1].y, _aclPoints[1].z);
Vector3<float> akF2(_aclPoints[2].x, _aclPoints[2].y, _aclPoints[2].z);
Triangle3<float> akTria(akF0, akF1, akF2);
DistSegment3Triangle3<float> akDistSegTria(akSeg, akTria);
return akDistSegTria.Get();
}
float MeshGeomFacet::DistanceToPoint (const Base::Vector3f &rclPt, Base::Vector3f &rclNt) const
{
Vector3<float> akPt(rclPt.x, rclPt.y, rclPt.z);
Vector3<float> akF0(_aclPoints[0].x, _aclPoints[0].y, _aclPoints[0].z);
Vector3<float> akF1(_aclPoints[1].x, _aclPoints[1].y, _aclPoints[1].z);
Vector3<float> akF2(_aclPoints[2].x, _aclPoints[2].y, _aclPoints[2].z);
Triangle3<float> akTria(akF0, akF1, akF2);
DistVector3Triangle3<float> akDistPtTria(akPt, akTria);
float fDist = akDistPtTria.Get();
// get nearest point of the facet
Vector3<float> akNt = akDistPtTria.GetClosestPoint1();
rclNt.Set(akNt.X(), akNt.Y(), akNt.Z());
return fDist;
}
void MeshGeomFacet::SubSample (float fStep, std::vector<Base::Vector3f> &rclPoints) const
{
std::vector<Base::Vector3f> clPoints;
Base::Vector3f A = _aclPoints[0];
Base::Vector3f B = _aclPoints[1];
Base::Vector3f C = _aclPoints[2];
Base::Vector3f clVecAB(B - A);
Base::Vector3f clVecAC(C - A);
Base::Vector3f clVecBC(C - B);
// laengste Achse entspricht AB
float fLenAB = clVecAB.Length();
float fLenAC = clVecAC.Length();
float fLenBC = clVecBC.Length();
if (fLenAC > fLenAB)
{
std::swap(B, C);
std::swap(fLenAB, fLenAC);
}
if (fLenBC > fLenAB)
{
std::swap(A, C);
std::swap(fLenBC, fLenAB);
}
clVecAB = (B - A);
clVecAC = (C - A);
clVecBC = (C - B);
Base::Vector3f clVecABNorm(clVecAB);
Base::Vector3f clVecHNorm((clVecAB % clVecAC) % clVecAB);
clVecABNorm.Normalize();
clVecHNorm.Normalize();
float bx = fLenAB;
float cy = float(sin(clVecAB.GetAngle(clVecAC)) * fLenAC);
float cx = float(sqrt(fabs(fLenAC * fLenAC - cy * cy)));
float fDetABC = bx*cy;
for (float px = (fStep / 2.0f); px < fLenAB; px += fStep)
{
for (float py = (fStep / 2.0f); py < cy; py += fStep)
{
float u = (bx*cy + cx*py - px*cy - bx*py) / fDetABC;
float v = (px*cy - cx*py) / fDetABC;
float w = (bx*py) / fDetABC;
if ((u >= 0.0f) && (v >= 0.0f) && (w >= 0.0f) && ((u + v) < 1.0f))
{
// rclPoints.push_back(CBase::Vector3f(u*A + v*B + w*C));
Base::Vector3f clV = A + (px * clVecABNorm) + (py * clVecHNorm);
clPoints.push_back(clV);
}
else
break;
}
}
// if couldn't subsample the facet take gravity center
if (clPoints.size() == 0)
clPoints.push_back(this->GetGravityPoint());
rclPoints.insert(rclPoints.end(), clPoints.begin(), clPoints.end());
}
/**
* Fast Triangle-Triangle Intersection Test by Tomas Moeller
* http://www.acm.org/jgt/papers/Moller97/tritri.html
* http://www.cs.lth.se/home/Tomas_Akenine_Moller/code/
*/
bool MeshGeomFacet::IntersectWithFacet(const MeshGeomFacet &rclFacet) const
{
float V[3][3], U[3][3];
for (int i = 0; i < 3; i++)
{
V[i][0] = _aclPoints[i].x;
V[i][1] = _aclPoints[i].y;
V[i][2] = _aclPoints[i].z;
U[i][0] = rclFacet._aclPoints[i].x;
U[i][1] = rclFacet._aclPoints[i].y;
U[i][2] = rclFacet._aclPoints[i].z;
}
if (tri_tri_intersect(V[0], V[1], V[2], U[0], U[1], U[2]) == 0)
return false; // no intersections
return true;
}
/**
* Fast Triangle-Triangle Intersection Test by Tomas Moeller
* http://www.acm.org/jgt/papers/Moller97/tritri.html
* http://www.cs.lth.se/home/Tomas_Akenine_Moller/code/
*/
int MeshGeomFacet::IntersectWithFacet (const MeshGeomFacet& rclFacet,
Base::Vector3f& rclPt0,
Base::Vector3f& rclPt1) const
{
float V[3][3], U[3][3];
int coplanar = 0;
float isectpt1[3], isectpt2[3];
for (int i = 0; i < 3; i++)
{
V[i][0] = _aclPoints[i].x;
V[i][1] = _aclPoints[i].y;
V[i][2] = _aclPoints[i].z;
U[i][0] = rclFacet._aclPoints[i].x;
U[i][1] = rclFacet._aclPoints[i].y;
U[i][2] = rclFacet._aclPoints[i].z;
}
if (tri_tri_intersect_with_isectline(V[0], V[1], V[2], U[0], U[1], U[2],
&coplanar, isectpt1, isectpt2) == 0)
return 0; // no intersections
rclPt0.x = isectpt1[0]; rclPt0.y = isectpt1[1]; rclPt0.z = isectpt1[2];
rclPt1.x = isectpt2[0]; rclPt1.y = isectpt2[1]; rclPt1.z = isectpt2[2];
// With extremely acute-angled triangles it may happen that the algorithm
// claims an intersection but the intersection points are far outside the
// model. So, a plausibility check is to verify that the intersection points
// are inside the bounding boxes of both triangles.
Base::BoundBox3f box1 = this->GetBoundBox();
if (!box1.IsInBox(rclPt0) || !box1.IsInBox(rclPt1))
return 0;
Base::BoundBox3f box2 = rclFacet.GetBoundBox();
if (!box2.IsInBox(rclPt0) || !box2.IsInBox(rclPt1))
return 0;
// Note: The algorithm delivers sometimes false-positives, i.e. it claims
// that the two triangles intersect but they don't. It seems that this bad
// behaviour occurs if the triangles are nearly co-planar
float mult = fabs(this->GetNormal() * rclFacet.GetNormal());
if (rclPt0 == rclPt1) {
if (mult < 0.995f) // not co-planar, thus no test needed
return 1;
if (this->IsPointOf(rclPt0) && rclFacet.IsPointOf(rclPt0))
return 1;
}
else {
if (mult < 0.995f) // not co-planar, thus no test needed
return 2;
if (this->IsPointOf(rclPt0) && rclFacet.IsPointOf(rclPt0) &&
this->IsPointOf(rclPt1) && rclFacet.IsPointOf(rclPt1))
return 2;
}
// the intersection algorithm delivered a false-positive
return 0;
}
bool MeshGeomFacet::IsPointOf (const Base::Vector3f &P) const
{
Base::Vector3d p1 = Base::convertTo<Base::Vector3d>(this->_aclPoints[0]);
Base::Vector3d p2 = Base::convertTo<Base::Vector3d>(this->_aclPoints[1]);
Base::Vector3d p3 = Base::convertTo<Base::Vector3d>(this->_aclPoints[2]);
Base::Vector3d p4 = Base::convertTo<Base::Vector3d>(P);
Base::Vector3d u = p2 - p1;
Base::Vector3d v = p3 - p1;
Base::Vector3d w = p4 - p1;
double uu = u * u;
double uv = u * v;
double vv = v * v;
double wu = w * u;
double wv = w * v;
double det = fabs((uu * vv) - (uv * uv));
// Note: Due to roundoff errors it can happen that we get very small
// negative values for s or t. This e.g. can happen if the point lies
// at the border of the facet. And as det could also become very small
// we need an adaptive tolerance.
const double eps=std::min<double>(1.0e-6, det*det);
double s = (vv * wu) - (uv * wv);
double t = (uu * wv) - (uv * wu);
// is the point inside the triangle?
if ((s >= -eps) && (t >= -eps) && ((s + t) <= det+eps)) {
return true;
}
return false;
}
float MeshGeomFacet::CenterOfInscribedCircle(Base::Vector3f& rclCenter) const
{
const Base::Vector3f& p0 = _aclPoints[0];
const Base::Vector3f& p1 = _aclPoints[1];
const Base::Vector3f& p2 = _aclPoints[2];
float a = Base::Distance(p1,p2);
float b = Base::Distance(p2,p0);
float c = Base::Distance(p0,p1);
// radius of the circle
float fRadius = Area();
fRadius *= 2.0f/(a + b + c);
// center of the circle
float w = a + b + c;
rclCenter.x = (a*p0.x + b*p1.x + c*p2.x)/w;
rclCenter.y = (a*p0.y + b*p1.y + c*p2.y)/w;
rclCenter.z = (a*p0.z + b*p1.z + c*p2.z)/w;
return fRadius;
}
float MeshGeomFacet::CenterOfCircumCircle(Base::Vector3f& rclCenter) const
{
const Base::Vector3f& p0 = _aclPoints[0];
const Base::Vector3f& p1 = _aclPoints[1];
const Base::Vector3f& p2 = _aclPoints[2];
Base::Vector3f u = (p1-p0);
Base::Vector3f v = (p2-p1);
Base::Vector3f w = (p0-p2);
float uu = (u * u);
float vv = (v * v);
float ww = (w * w);
float uv = - (u * v);
float vw = - (v * w);
float uw = - (w * u);
float w0 = static_cast<float>(2 * sqrt(uu * ww - uw * uw) * uw / (uu * ww));
float w1 = static_cast<float>(2 * sqrt(uu * vv - uv * uv) * uv / (uu * vv));
float w2 = static_cast<float>(2 * sqrt(vv * ww - vw * vw) * vw / (vv * ww));
// center of the circle
float wx = w0 + w1 + w2;
rclCenter.x = (w0*p0.x + w1*p1.x + w2*p2.x)/wx;
rclCenter.y = (w0*p0.y + w1*p1.y + w2*p2.y)/wx;
rclCenter.z = (w0*p0.z + w1*p1.z + w2*p2.z)/wx;
// radius of the circle
float fRadius = static_cast<float>(sqrt(uu * vv * ww) / (4 * Area()));
return fRadius;
}
unsigned short MeshGeomFacet::NearestEdgeToPoint(const Base::Vector3f& rclPt) const
{
unsigned short usSide;
const Base::Vector3f& rcP1 = _aclPoints[0];
const Base::Vector3f& rcP2 = _aclPoints[1];
const Base::Vector3f& rcP3 = _aclPoints[2];
float fD1 = FLOAT_MAX;
float fD2 = FLOAT_MAX;
float fD3 = FLOAT_MAX;
// 1st edge
Base::Vector3f clDir = rcP2 - rcP1;
float fLen = Base::Distance(rcP2, rcP1);
float t = ( ( rclPt - rcP1 ) * clDir ) / ( fLen * fLen );
if ( t < 0.0f )
fD1 = Base::Distance(rclPt, rcP1);
else if ( t > 1.0f )
fD1 = Base::Distance(rclPt, rcP2);
else
fD1 = ( ( ( rclPt - rcP1 ) % clDir).Length() ) / fLen;
// 2nd edge
clDir = rcP3 - rcP2;
fLen = Base::Distance(rcP3, rcP2);
t = ( ( rclPt - rcP2 ) * clDir ) / ( fLen * fLen );
if ( t < 0.0f )
fD2 = Base::Distance(rclPt, rcP2);
else if ( t > 1.0f )
fD2 = Base::Distance(rclPt, rcP3);
else
fD2 = ( ( ( rclPt - rcP2 ) % clDir).Length() ) / fLen;
// 3rd edge
clDir = rcP1 - rcP3;
fLen = Base::Distance(rcP1, rcP3);
t = ( ( rclPt - rcP3 ) * clDir ) / ( fLen * fLen );
if ( t < 0.0f )
fD3 = Base::Distance(rclPt, rcP3);
else if ( t > 1.0f )
fD3 = Base::Distance(rclPt, rcP1);
else
fD3 = ( ( ( rclPt - rcP3 ) % clDir).Length() ) / fLen;
if ( fD1 < fD2 )
{
if ( fD1 < fD3 )
{
usSide = 0;
}
else
{
usSide = 2;
}
}
else
{
if ( fD2 < fD3 )
{
usSide = 1;
}
else
{
usSide = 2;
}
}
return usSide;
}
void MeshGeomFacet::NearestEdgeToPoint(const Base::Vector3f& rclPt, float& fDistance, unsigned short& usSide) const
{
const Base::Vector3f& rcP1 = _aclPoints[0];
const Base::Vector3f& rcP2 = _aclPoints[1];
const Base::Vector3f& rcP3 = _aclPoints[2];
float fD1 = FLOAT_MAX;
float fD2 = FLOAT_MAX;
float fD3 = FLOAT_MAX;
// 1st edge
Base::Vector3f clDir = rcP2 - rcP1;
float fLen = Base::Distance(rcP2, rcP1);
float t = ( ( rclPt - rcP1 ) * clDir ) / ( fLen * fLen );
if ( t < 0.0f )
fD1 = Base::Distance(rclPt, rcP1);
else if ( t > 1.0f )
fD1 = Base::Distance(rclPt, rcP2);
else
fD1 = ( ( ( rclPt - rcP1 ) % clDir).Length() ) / fLen;
// 2nd edge
clDir = rcP3 - rcP2;
fLen = Base::Distance(rcP3, rcP2);
t = ( ( rclPt - rcP2 ) * clDir ) / ( fLen * fLen );
if ( t < 0.0f )
fD2 = Base::Distance(rclPt, rcP2);
else if ( t > 1.0f )
fD2 = Base::Distance(rclPt, rcP3);
else
fD2 = ( ( ( rclPt - rcP2 ) % clDir).Length() ) / fLen;
// 3rd edge
clDir = rcP1 - rcP3;
fLen = Base::Distance(rcP1, rcP3);
t = ( ( rclPt - rcP3 ) * clDir ) / ( fLen * fLen );
if ( t < 0.0f )
fD3 = Base::Distance(rclPt, rcP3);
else if ( t > 1.0f )
fD3 = Base::Distance(rclPt, rcP1);
else
fD3 = ( ( ( rclPt - rcP3 ) % clDir).Length() ) / fLen;
if ( fD1 < fD2 )
{
if ( fD1 < fD3 )
{
usSide = 0;
fDistance = fD1;
}
else
{
usSide = 2;
fDistance = fD3;
}
}
else
{
if ( fD2 < fD3 )
{
usSide = 1;
fDistance = fD2;
}
else
{
usSide = 2;
fDistance = fD3;
}
}
}
float MeshGeomFacet::VolumeOfPrism (const MeshGeomFacet& rclF1) const
{
Base::Vector3f P1 = this->_aclPoints[0];
Base::Vector3f P2 = this->_aclPoints[1];
Base::Vector3f P3 = this->_aclPoints[2];
Base::Vector3f Q1 = rclF1._aclPoints[0];
Base::Vector3f Q2 = rclF1._aclPoints[1];
Base::Vector3f Q3 = rclF1._aclPoints[2];
if ((P1-Q2).Length() < (P1-Q1).Length())
{
Base::Vector3f tmp = Q1;
Q1 = Q2;
Q2 = tmp;
}
if ((P1-Q3).Length() < (P1-Q1).Length())
{
Base::Vector3f tmp = Q1;
Q1 = Q3;
Q3 = tmp;
}
if ((P2-Q3).Length() < (P2-Q2).Length())
{
Base::Vector3f tmp = Q2;
Q2 = Q3;
Q3 = tmp;
}
Base::Vector3f N1 = (P2-P1) % (P3-P1);
Base::Vector3f N2 = (P2-P1) % (Q2-P1);
Base::Vector3f N3 = (Q2-P1) % (Q1-P1);
float fVol=0.0f;
fVol += float(fabs((Q3-P1) * N1));
fVol += float(fabs((Q3-P1) * N2));
fVol += float(fabs((Q3-P1) * N3));
fVol /= 6.0f;
return fVol;;
}
float MeshGeomFacet::MaximumAngle () const
{
float fMaxAngle = 0.0f;
for ( int i=0; i<3; i++ ) {
Base::Vector3f dir1(_aclPoints[(i+1)%3]-_aclPoints[i]);
Base::Vector3f dir2(_aclPoints[(i+2)%3]-_aclPoints[i]);
float fAngle = dir1.GetAngle(dir2);
if (fAngle > fMaxAngle)
fMaxAngle = fAngle;
}
return fMaxAngle;
}
float MeshGeomFacet::MinimumAngle () const
{
float fMinAngle = Mathf::PI;
for ( int i=0; i<3; i++ ) {
Base::Vector3f dir1(_aclPoints[(i+1)%3]-_aclPoints[i]);
Base::Vector3f dir2(_aclPoints[(i+2)%3]-_aclPoints[i]);
float fAngle = dir1.GetAngle(dir2);
if (fAngle < fMinAngle)
fMinAngle = fAngle;
}
return fMinAngle;
}
bool MeshGeomFacet::IsPointOfSphere(const Base::Vector3f& rP) const
{
float radius;
Base::Vector3f center;
radius = CenterOfCircumCircle(center);
radius *= radius;
float dist = Base::DistanceP2(rP, center);
return dist < radius;
}
bool MeshGeomFacet::IsPointOfSphere(const MeshGeomFacet& rFacet) const
{
float radius;
Base::Vector3f center;
radius = CenterOfCircumCircle(center);
radius *= radius;
for (int i=0; i<3; i++) {
float dist = Base::DistanceP2(rFacet._aclPoints[i], center);
if (dist < radius)
return true;
}
return false;
}
float MeshGeomFacet::AspectRatio() const
{
Base::Vector3f d0 = _aclPoints[0] - _aclPoints[1];
Base::Vector3f d1 = _aclPoints[1] - _aclPoints[2];
Base::Vector3f d2 = _aclPoints[2] - _aclPoints[0];
float l2, maxl2 = d0.Sqr();
if ((l2=d1.Sqr()) > maxl2)
maxl2 = l2;
d1 = d2;
if ((l2=d1.Sqr()) > maxl2)
maxl2 = l2;
// squared area of the parallelogram spanned by d0 and d1
float a2 = (d0 % d1).Sqr();
return float(sqrt( (maxl2 * maxl2) / a2 ));
}
float MeshGeomFacet::AspectRatio2() const
{
const Base::Vector3f& rcP1 = _aclPoints[0];
const Base::Vector3f& rcP2 = _aclPoints[1];
const Base::Vector3f& rcP3 = _aclPoints[2];
float a = Base::Distance(rcP1, rcP2);
float b = Base::Distance(rcP2, rcP3);
float c = Base::Distance(rcP3, rcP1);
// https://stackoverflow.com/questions/10289752/aspect-ratio-of-a-triangle-of-a-meshed-surface
return a * b * c / ((b + c - a) * (c + a - b) * (a + b - c));
}
float MeshGeomFacet::Roundness() const
{
const double FOUR_ROOT3 = 6.928203230275509;
double area = static_cast<double>(Area());
Base::Vector3f d0 = _aclPoints[0] - _aclPoints[1];
Base::Vector3f d1 = _aclPoints[1] - _aclPoints[2];
Base::Vector3f d2 = _aclPoints[2] - _aclPoints[0];
double sum = static_cast<double>(d0.Sqr() + d1.Sqr() + d2.Sqr());
return static_cast<float>(FOUR_ROOT3 * area / sum);
}
void MeshGeomFacet::Transform(const Base::Matrix4D& mat)
{
mat.multVec(_aclPoints[0], _aclPoints[0]);
mat.multVec(_aclPoints[1], _aclPoints[1]);
mat.multVec(_aclPoints[2], _aclPoints[2]);
NormalInvalid();
}
Reference in New Issue View Git Blame Copy Permalink