diff --git a/src/Mod/Mesh/App/CMakeLists.txt b/src/Mod/Mesh/App/CMakeLists.txt index 8934480b0a..cc3d4d4ff7 100644 --- a/src/Mod/Mesh/App/CMakeLists.txt +++ b/src/Mod/Mesh/App/CMakeLists.txt @@ -87,6 +87,10 @@ SOURCE_GROUP("Core" FILES ${Core_SRCS}) SET(WildMagic4_SRCS WildMagic4/Wm4ApprCylinderFit3.cpp WildMagic4/Wm4ApprCylinderFit3.h + WildMagic4/Wm4ApprGaussPointsFit2.cpp + WildMagic4/Wm4ApprGaussPointsFit2.h + WildMagic4/Wm4ApprGaussPointsFit3.cpp + WildMagic4/Wm4ApprGaussPointsFit3.h WildMagic4/Wm4ApprLineFit3.cpp WildMagic4/Wm4ApprLineFit3.h WildMagic4/Wm4ApprPlaneFit3.cpp @@ -99,8 +103,22 @@ SET(WildMagic4_SRCS WildMagic4/Wm4ApprSphereFit3.h WildMagic4/Wm4BandedMatrix.h WildMagic4/Wm4BandedMatrix.inl + WildMagic4/Wm4Box2.h + WildMagic4/Wm4Box2.inl WildMagic4/Wm4Box3.h WildMagic4/Wm4Box3.inl + WildMagic4/Wm4ContBox2.cpp + WildMagic4/Wm4ContBox2.h + WildMagic4/Wm4ContBox3.cpp + WildMagic4/Wm4ContBox3.h + WildMagic4/Wm4ConvexHull.cpp + WildMagic4/Wm4ConvexHull.h + WildMagic4/Wm4ConvexHull1.cpp + WildMagic4/Wm4ConvexHull1.h + WildMagic4/Wm4ConvexHull2.cpp + WildMagic4/Wm4ConvexHull2.h + WildMagic4/Wm4ConvexHull3.cpp + WildMagic4/Wm4ConvexHull3.h WildMagic4/Wm4DelPolygonEdge.cpp WildMagic4/Wm4DelPolygonEdge.h WildMagic4/Wm4DelPolyhedronFace.cpp @@ -207,6 +225,9 @@ SET(WildMagic4_SRCS WildMagic4/Wm4Polynomial1.inl WildMagic4/Wm4QuadricSurface.cpp WildMagic4/Wm4QuadricSurface.h + WildMagic4/Wm4Quaternion.cpp + WildMagic4/Wm4Quaternion.h + WildMagic4/Wm4Quaternion.inl WildMagic4/Wm4Query.h WildMagic4/Wm4Query.inl WildMagic4/Wm4Query2.h diff --git a/src/Mod/Mesh/App/WildMagic4/Wm4ApprGaussPointsFit2.cpp b/src/Mod/Mesh/App/WildMagic4/Wm4ApprGaussPointsFit2.cpp new file mode 100644 index 0000000000..2488feffc5 --- /dev/null +++ b/src/Mod/Mesh/App/WildMagic4/Wm4ApprGaussPointsFit2.cpp @@ -0,0 +1,73 @@ +// Geometric Tools, LLC +// Copyright (c) 1998-2010 +// Distributed under the Boost Software License, Version 1.0. +// http://www.boost.org/LICENSE_1_0.txt +// http://www.geometrictools.com/License/Boost/LICENSE_1_0.txt +// +// File Version: 4.10.0 (2009/11/18) + +#include "Wm4FoundationPCH.h" +#include "Wm4ApprGaussPointsFit2.h" +#include "Wm4Eigen.h" + +namespace Wm4 +{ +//---------------------------------------------------------------------------- +template +Box2 GaussPointsFit2 (int iQuantity, const Vector2* akPoint) +{ + Box2 kBox(Vector2::ZERO,Vector2::UNIT_X, + Vector2::UNIT_Y,(Real)1.0,(Real)1.0); + + // compute the mean of the points + kBox.Center = akPoint[0]; + int i; + for (i = 1; i < iQuantity; i++) + { + kBox.Center += akPoint[i]; + } + Real fInvQuantity = ((Real)1.0)/iQuantity; + kBox.Center *= fInvQuantity; + + // compute the covariance matrix of the points + Real fSumXX = (Real)0.0, fSumXY = (Real)0.0, fSumYY = (Real)0.0; + for (i = 0; i < iQuantity; i++) + { + Vector2 kDiff = akPoint[i] - kBox.Center; + fSumXX += kDiff.X()*kDiff.X(); + fSumXY += kDiff.X()*kDiff.Y(); + fSumYY += kDiff.Y()*kDiff.Y(); + } + + fSumXX *= fInvQuantity; + fSumXY *= fInvQuantity; + fSumYY *= fInvQuantity; + + // setup the eigensolver + Eigen kES(2); + kES(0,0) = fSumXX; + kES(0,1) = fSumXY; + kES(1,0) = fSumXY; + kES(1,1) = fSumYY; + kES.IncrSortEigenStuff2(); + + for (i = 0; i < 2; i++) + { + kBox.Extent[i] = kES.GetEigenvalue(i); + kES.GetEigenvector(i,kBox.Axis[i]); + } + + return kBox; +} +//---------------------------------------------------------------------------- + +//---------------------------------------------------------------------------- +// explicit instantiation +//---------------------------------------------------------------------------- +template WM4_FOUNDATION_ITEM +Box2 GaussPointsFit2 (int, const Vector2*); + +template WM4_FOUNDATION_ITEM +Box2 GaussPointsFit2 (int, const Vector2*); +//---------------------------------------------------------------------------- +} diff --git a/src/Mod/Mesh/App/WildMagic4/Wm4ApprGaussPointsFit2.h b/src/Mod/Mesh/App/WildMagic4/Wm4ApprGaussPointsFit2.h new file mode 100644 index 0000000000..ce9e8b6aa8 --- /dev/null +++ b/src/Mod/Mesh/App/WildMagic4/Wm4ApprGaussPointsFit2.h @@ -0,0 +1,28 @@ +// Geometric Tools, LLC +// Copyright (c) 1998-2010 +// Distributed under the Boost Software License, Version 1.0. +// http://www.boost.org/LICENSE_1_0.txt +// http://www.geometrictools.com/License/Boost/LICENSE_1_0.txt +// +// File Version: 4.10.0 (2009/11/18) + +#ifndef WM4APPRGAUSSPOINTSFIT2_H +#define WM4APPRGAUSSPOINTSFIT2_H + +#include "Wm4FoundationLIB.h" +#include "Wm4Box2.h" + +namespace Wm4 +{ + +// Fit points with a Gaussian distribution. The center is the mean of the +// points, the axes are the eigenvectors of the covariance matrix, and the +// extents are the eigenvalues of the covariance matrix and are returned in +// increasing order. The quantites are stored in a Box2 just to have a +// single container. +template WM4_FOUNDATION_ITEM +Box2 GaussPointsFit2 (int iQuantity, const Vector2* akPoint); + +} + +#endif diff --git a/src/Mod/Mesh/App/WildMagic4/Wm4ApprGaussPointsFit3.cpp b/src/Mod/Mesh/App/WildMagic4/Wm4ApprGaussPointsFit3.cpp new file mode 100644 index 0000000000..41a930b95f --- /dev/null +++ b/src/Mod/Mesh/App/WildMagic4/Wm4ApprGaussPointsFit3.cpp @@ -0,0 +1,86 @@ +// Geometric Tools, LLC +// Copyright (c) 1998-2010 +// Distributed under the Boost Software License, Version 1.0. +// http://www.boost.org/LICENSE_1_0.txt +// http://www.geometrictools.com/License/Boost/LICENSE_1_0.txt +// +// File Version: 4.10.0 (2009/11/18) + +#include "Wm4FoundationPCH.h" +#include "Wm4ApprGaussPointsFit3.h" +#include "Wm4Eigen.h" + +namespace Wm4 +{ +//---------------------------------------------------------------------------- +template +Box3 GaussPointsFit3 (int iQuantity, const Vector3* akPoint) +{ + Box3 kBox(Vector3::ZERO,Vector3::UNIT_X, + Vector3::UNIT_Y,Vector3::UNIT_Z,(Real)1.0,(Real)1.0, + (Real)1.0); + + // compute the mean of the points + kBox.Center = akPoint[0]; + int i; + for (i = 1; i < iQuantity; i++) + { + kBox.Center += akPoint[i]; + } + Real fInvQuantity = ((Real)1.0)/iQuantity; + kBox.Center *= fInvQuantity; + + // compute the covariance matrix of the points + Real fSumXX = (Real)0.0, fSumXY = (Real)0.0, fSumXZ = (Real)0.0; + Real fSumYY = (Real)0.0, fSumYZ = (Real)0.0, fSumZZ = (Real)0.0; + for (i = 0; i < iQuantity; i++) + { + Vector3 kDiff = akPoint[i] - kBox.Center; + fSumXX += kDiff.X()*kDiff.X(); + fSumXY += kDiff.X()*kDiff.Y(); + fSumXZ += kDiff.X()*kDiff.Z(); + fSumYY += kDiff.Y()*kDiff.Y(); + fSumYZ += kDiff.Y()*kDiff.Z(); + fSumZZ += kDiff.Z()*kDiff.Z(); + } + + fSumXX *= fInvQuantity; + fSumXY *= fInvQuantity; + fSumXZ *= fInvQuantity; + fSumYY *= fInvQuantity; + fSumYZ *= fInvQuantity; + fSumZZ *= fInvQuantity; + + // setup the eigensolver + Eigen kES(3); + kES(0,0) = fSumXX; + kES(0,1) = fSumXY; + kES(0,2) = fSumXZ; + kES(1,0) = fSumXY; + kES(1,1) = fSumYY; + kES(1,2) = fSumYZ; + kES(2,0) = fSumXZ; + kES(2,1) = fSumYZ; + kES(2,2) = fSumZZ; + kES.IncrSortEigenStuff3(); + + for (i = 0; i < 3; i++) + { + kBox.Extent[i] = kES.GetEigenvalue(i); + kES.GetEigenvector(i,kBox.Axis[i]); + } + + return kBox; +} +//---------------------------------------------------------------------------- + +//---------------------------------------------------------------------------- +// explicit instantiation +//---------------------------------------------------------------------------- +template WM4_FOUNDATION_ITEM +Box3 GaussPointsFit3 (int, const Vector3*); + +template WM4_FOUNDATION_ITEM +Box3 GaussPointsFit3 (int, const Vector3*); +//---------------------------------------------------------------------------- +} diff --git a/src/Mod/Mesh/App/WildMagic4/Wm4ApprGaussPointsFit3.h b/src/Mod/Mesh/App/WildMagic4/Wm4ApprGaussPointsFit3.h new file mode 100644 index 0000000000..ae75457177 --- /dev/null +++ b/src/Mod/Mesh/App/WildMagic4/Wm4ApprGaussPointsFit3.h @@ -0,0 +1,28 @@ +// Geometric Tools, LLC +// Copyright (c) 1998-2010 +// Distributed under the Boost Software License, Version 1.0. +// http://www.boost.org/LICENSE_1_0.txt +// http://www.geometrictools.com/License/Boost/LICENSE_1_0.txt +// +// File Version: 4.10.0 (2009/11/18) + +#ifndef WM4APPRGAUSSPOINTSFIT3_H +#define WM4APPRGAUSSPOINTSFIT3_H + +#include "Wm4FoundationLIB.h" +#include "Wm4Box3.h" + +namespace Wm4 +{ + +// Fit points with a Gaussian distribution. The center is the mean of the +// points, the axes are the eigenvectors of the covariance matrix, and the +// extents are the eigenvalues of the covariance matrix and are returned in +// increasing order. The quantites are stored in a Box3 just to have a +// single container. +template WM4_FOUNDATION_ITEM +Box3 GaussPointsFit3 (int iQuantity, const Vector3* akPoint); + +} + +#endif diff --git a/src/Mod/Mesh/App/WildMagic4/Wm4Box2.h b/src/Mod/Mesh/App/WildMagic4/Wm4Box2.h new file mode 100644 index 0000000000..be2462747a --- /dev/null +++ b/src/Mod/Mesh/App/WildMagic4/Wm4Box2.h @@ -0,0 +1,48 @@ +// Geometric Tools, LLC +// Copyright (c) 1998-2010 +// Distributed under the Boost Software License, Version 1.0. +// http://www.boost.org/LICENSE_1_0.txt +// http://www.geometrictools.com/License/Boost/LICENSE_1_0.txt +// +// File Version: 4.10.0 (2009/11/18) + +#ifndef WM4BOX2_H +#define WM4BOX2_H + +#include "Wm4FoundationLIB.h" +#include "Wm4Vector2.h" + +namespace Wm4 +{ + +template +class Box2 +{ +public: + // A box has center C, axis directions U[0] and U[1] (both unit-length + // vectors), and extents e[0] and e[1] (both nonnegative numbers). A + // point X = C+y[0]*U[0]+y[1]*U[1] is inside or on the box whenever + // |y[i]| <= e[i] for all i. + + // construction + Box2 (); // uninitialized + Box2 (const Vector2& rkCenter, const Vector2* akAxis, + const Real* afExtent); + Box2 (const Vector2& rkCenter, const Vector2& rkAxis0, + const Vector2& rkAxis1, Real fExtent0, Real fExtent1); + + void ComputeVertices (Vector2 akVertex[4]) const; + + Vector2 Center; + Vector2 Axis[2]; // must be an orthonormal set of vectors + Real Extent[2]; // must be nonnegative +}; + +#include "Wm4Box2.inl" + +typedef Box2 Box2f; +typedef Box2 Box2d; + +} + +#endif diff --git a/src/Mod/Mesh/App/WildMagic4/Wm4Box2.inl b/src/Mod/Mesh/App/WildMagic4/Wm4Box2.inl new file mode 100644 index 0000000000..3d16f8a227 --- /dev/null +++ b/src/Mod/Mesh/App/WildMagic4/Wm4Box2.inl @@ -0,0 +1,55 @@ +// Geometric Tools, LLC +// Copyright (c) 1998-2010 +// Distributed under the Boost Software License, Version 1.0. +// http://www.boost.org/LICENSE_1_0.txt +// http://www.geometrictools.com/License/Boost/LICENSE_1_0.txt +// +// File Version: 4.10.0 (2009/11/18) + +//---------------------------------------------------------------------------- +template +Box2::Box2 () +{ + // uninitialized +} +//---------------------------------------------------------------------------- +template +Box2::Box2 (const Vector2& rkCenter, const Vector2* akAxis, + const Real* afExtent) + : + Center(rkCenter) +{ + for (int i = 0; i < 2; i++) + { + Axis[i] = akAxis[i]; + Extent[i] = afExtent[i]; + } +} +//---------------------------------------------------------------------------- +template +Box2::Box2 (const Vector2& rkCenter, const Vector2& rkAxis0, + const Vector2& rkAxis1, Real fExtent0, Real fExtent1) + : + Center(rkCenter) +{ + Axis[0] = rkAxis0; + Axis[1] = rkAxis1; + Extent[0] = fExtent0; + Extent[1] = fExtent1; +} +//---------------------------------------------------------------------------- +template +void Box2::ComputeVertices (Vector2 akVertex[4]) const +{ + Vector2 akEAxis[2] = + { + Axis[0]*Extent[0], + Axis[1]*Extent[1] + }; + + akVertex[0] = Center - akEAxis[0] - akEAxis[1]; + akVertex[1] = Center + akEAxis[0] - akEAxis[1]; + akVertex[2] = Center + akEAxis[0] + akEAxis[1]; + akVertex[3] = Center - akEAxis[0] + akEAxis[1]; +} +//---------------------------------------------------------------------------- diff --git a/src/Mod/Mesh/App/WildMagic4/Wm4ContBox2.cpp b/src/Mod/Mesh/App/WildMagic4/Wm4ContBox2.cpp new file mode 100644 index 0000000000..0c71ff686c --- /dev/null +++ b/src/Mod/Mesh/App/WildMagic4/Wm4ContBox2.cpp @@ -0,0 +1,545 @@ +// Geometric Tools, LLC +// Copyright (c) 1998-2010 +// Distributed under the Boost Software License, Version 1.0. +// http://www.boost.org/LICENSE_1_0.txt +// http://www.geometrictools.com/License/Boost/LICENSE_1_0.txt +// +// File Version: 4.10.0 (2009/11/18) + +#include "Wm4FoundationPCH.h" +#include "Wm4ContBox2.h" +#include "Wm4ApprGaussPointsFit2.h" +#include "Wm4ConvexHull2.h" + +namespace Wm4 +{ +//---------------------------------------------------------------------------- +template +Box2 ContAlignedBox (int iQuantity, const Vector2* akPoint) +{ + Vector2 kMin, kMax; + Vector2::ComputeExtremes(iQuantity,akPoint,kMin,kMax); + + Box2 kBox; + kBox.Center = ((Real)0.5)*(kMin + kMax); + kBox.Axis[0] = Vector2::UNIT_X; + kBox.Axis[1] = Vector2::UNIT_Y; + Vector2 kHalfDiagonal = ((Real)0.5)*(kMax - kMin); + for (int i = 0; i < 2; i++) + { + kBox.Extent[i] = kHalfDiagonal[i]; + } + + return kBox; +} +//---------------------------------------------------------------------------- +template +Box2 ContOrientedBox (int iQuantity, const Vector2* akPoint) +{ + Box2 kBox = GaussPointsFit2(iQuantity,akPoint); + + // Let C be the box center and let U0 and U1 be the box axes. Each + // input point is of the form X = C + y0*U0 + y1*U1. The following code + // computes min(y0), max(y0), min(y1), max(y1), min(y2), and max(y2). + // The box center is then adjusted to be + // C' = C + 0.5*(min(y0)+max(y0))*U0 + 0.5*(min(y1)+max(y1))*U1 + + Vector2 kDiff = akPoint[0] - kBox.Center; + Vector2 kMin(kDiff.Dot(kBox.Axis[0]),kDiff.Dot(kBox.Axis[1])); + Vector2 kMax = kMin; + for (int i = 1; i < iQuantity; i++) + { + kDiff = akPoint[i] - kBox.Center; + for (int j = 0; j < 2; j++) + { + Real fDot = kDiff.Dot(kBox.Axis[j]); + if (fDot < kMin[j]) + { + kMin[j] = fDot; + } + else if (fDot > kMax[j]) + { + kMax[j] = fDot; + } + } + } + + kBox.Center += + (((Real)0.5)*(kMin[0]+kMax[0]))*kBox.Axis[0] + + (((Real)0.5)*(kMin[1]+kMax[1]))*kBox.Axis[1]; + + kBox.Extent[0] = ((Real)0.5)*(kMax[0] - kMin[0]); + kBox.Extent[1] = ((Real)0.5)*(kMax[1] - kMin[1]); + + return kBox; +} +//---------------------------------------------------------------------------- +template +static void UpdateBox (const Vector2& rkLPoint, + const Vector2& rkRPoint, const Vector2& rkBPoint, + const Vector2& rkTPoint, const Vector2& rkU, + const Vector2& rkV, Real& rfMinAreaDiv4, Box2& rkBox) +{ + Vector2 kRLDiff = rkRPoint - rkLPoint; + Vector2 kTBDiff = rkTPoint - rkBPoint; + Real fExtent0 = ((Real)0.5)*(rkU.Dot(kRLDiff)); + Real fExtent1 = ((Real)0.5)*(rkV.Dot(kTBDiff)); + Real fAreaDiv4 = fExtent0*fExtent1; + if (fAreaDiv4 < rfMinAreaDiv4) + { + rfMinAreaDiv4 = fAreaDiv4; + rkBox.Axis[0] = rkU; + rkBox.Axis[1] = rkV; + rkBox.Extent[0] = fExtent0; + rkBox.Extent[1] = fExtent1; + Vector2 kLBDiff = rkLPoint - rkBPoint; + rkBox.Center = rkLPoint + rkU*fExtent0 + + rkV*(fExtent1 - rkV.Dot(kLBDiff)); + } +} +//---------------------------------------------------------------------------- +template +Box2 ContMinBox (int iQuantity, const Vector2* akPoint, + Real fEpsilon, Query::Type eQueryType, bool bIsConvexPolygon) +{ + Box2 kBox; + + // get the convex hull of the points + Vector2* akHPoint = 0; + if (bIsConvexPolygon) + { + akHPoint = (Vector2*)akPoint; + } + else + { + ConvexHull2 kHull(iQuantity,(Vector2*)akPoint,fEpsilon, + false,eQueryType); + int iHDim = kHull.GetDimension(); + int iHQuantity = kHull.GetSimplexQuantity(); + const int* aiHIndex = kHull.GetIndices(); + + if (iHDim == 0) + { + kBox.Center = akPoint[0]; + kBox.Axis[0] = Vector2::UNIT_X; + kBox.Axis[1] = Vector2::UNIT_Y; + kBox.Extent[0] = (Real)0.0; + kBox.Extent[1] = (Real)0.0; + return kBox; + } + + if (iHDim == 1) + { + ConvexHull1* pkHull1 = kHull.GetConvexHull1(); + aiHIndex = pkHull1->GetIndices(); + + kBox.Center = ((Real)0.5)*(akPoint[aiHIndex[0]] + + akPoint[aiHIndex[1]]); + Vector2 kDiff = akPoint[aiHIndex[1]] - akPoint[aiHIndex[0]]; + kBox.Extent[0] = ((Real)0.5)*kDiff.Normalize(); + kBox.Extent[1] = (Real)0.0; + kBox.Axis[0] = kDiff; + kBox.Axis[1] = -kBox.Axis[0].Perp(); + + WM4_DELETE pkHull1; + return kBox; + } + + iQuantity = iHQuantity; + akHPoint = WM4_NEW Vector2[iQuantity]; + for (int i = 0; i < iQuantity; i++) + { + akHPoint[i] = akPoint[aiHIndex[i]]; + } + } + + // The input points are V[0] through V[N-1] and are assumed to be the + // vertices of a convex polygon that are counterclockwise ordered. The + // input points must not contain three consecutive collinear points. + + // Unit-length edge directions of convex polygon. These could be + // precomputed and passed to this routine if the application requires it. + int iQuantityM1 = iQuantity -1; + Vector2* akEdge = WM4_NEW Vector2[iQuantity]; + bool* abVisited = WM4_NEW bool[iQuantity]; + int i; + for (i = 0; i < iQuantityM1; i++) + { + akEdge[i] = akHPoint[i+1] - akHPoint[i]; + akEdge[i].Normalize(); + abVisited[i] = false; + } + akEdge[iQuantityM1] = akHPoint[0] - akHPoint[iQuantityM1]; + akEdge[iQuantityM1].Normalize(); + abVisited[iQuantityM1] = false; + + // Find the smallest axis-aligned box containing the points. Keep track + // of the extremum indices, L (left), R (right), B (bottom), and T (top) + // so that the following constraints are met: + // V[L].X() <= V[i].X() for all i and V[(L+1)%N].X() > V[L].X() + // V[R].X() >= V[i].X() for all i and V[(R+1)%N].X() < V[R].X() + // V[B].Y() <= V[i].Y() for all i and V[(B+1)%N].Y() > V[B].Y() + // V[T].Y() >= V[i].Y() for all i and V[(T+1)%N].Y() < V[T].Y() + Real fXMin = akHPoint[0].X(), fXMax = fXMin; + Real fYMin = akHPoint[0].Y(), fYMax = fYMin; + int iLIndex = 0, iRIndex = 0, iBIndex = 0, iTIndex = 0; + for (i = 1; i < iQuantity; i++) + { + if (akHPoint[i].X() <= fXMin) + { + fXMin = akHPoint[i].X(); + iLIndex = i; + } + if (akHPoint[i].X() >= fXMax) + { + fXMax = akHPoint[i].X(); + iRIndex = i; + } + + if (akHPoint[i].Y() <= fYMin) + { + fYMin = akHPoint[i].Y(); + iBIndex = i; + } + if (akHPoint[i].Y() >= fYMax) + { + fYMax = akHPoint[i].Y(); + iTIndex = i; + } + } + + // wrap-around tests to ensure the constraints mentioned above + if (iLIndex == iQuantityM1) + { + if (akHPoint[0].X() <= fXMin) + { + fXMin = akHPoint[0].X(); + iLIndex = 0; + } + } + + if (iRIndex == iQuantityM1) + { + if (akHPoint[0].X() >= fXMax) + { + fXMax = akHPoint[0].X(); + iRIndex = 0; + } + } + + if (iBIndex == iQuantityM1) + { + if (akHPoint[0].Y() <= fYMin) + { + fYMin = akHPoint[0].Y(); + iBIndex = 0; + } + } + + if (iTIndex == iQuantityM1) + { + if (akHPoint[0].Y() >= fYMax) + { + fYMax = akHPoint[0].Y(); + iTIndex = 0; + } + } + + // dimensions of axis-aligned box (extents store width and height for now) + kBox.Center.X() = ((Real)0.5)*(fXMin + fXMax); + kBox.Center.Y() = ((Real)0.5)*(fYMin + fYMax); + kBox.Axis[0] = Vector2::UNIT_X; + kBox.Axis[1] = Vector2::UNIT_Y; + kBox.Extent[0] = ((Real)0.5)*(fXMax - fXMin); + kBox.Extent[1] = ((Real)0.5)*(fYMax - fYMin); + Real fMinAreaDiv4 = kBox.Extent[0]*kBox.Extent[1]; + + // rotating calipers algorithm + enum { F_NONE, F_LEFT, F_RIGHT, F_BOTTOM, F_TOP }; + Vector2 kU = Vector2::UNIT_X, kV = Vector2::UNIT_Y; + + bool bDone = false; + while (!bDone) + { + // determine edge that forms smallest angle with current box edges + int iFlag = F_NONE; + Real fMaxDot = (Real)0.0; + + Real fDot = kU.Dot(akEdge[iBIndex]); + if (fDot > fMaxDot) + { + fMaxDot = fDot; + iFlag = F_BOTTOM; + } + + fDot = kV.Dot(akEdge[iRIndex]); + if (fDot > fMaxDot) + { + fMaxDot = fDot; + iFlag = F_RIGHT; + } + + fDot = -kU.Dot(akEdge[iTIndex]); + if (fDot > fMaxDot) + { + fMaxDot = fDot; + iFlag = F_TOP; + } + + fDot = -kV.Dot(akEdge[iLIndex]); + if (fDot > fMaxDot) + { + fMaxDot = fDot; + iFlag = F_LEFT; + } + + switch (iFlag) + { + case F_BOTTOM: + if (abVisited[iBIndex]) + { + bDone = true; + } + else + { + // compute box axes with E[B] as an edge + kU = akEdge[iBIndex]; + kV = -kU.Perp(); + UpdateBox(akHPoint[iLIndex],akHPoint[iRIndex], + akHPoint[iBIndex],akHPoint[iTIndex],kU,kV,fMinAreaDiv4, + kBox); + + // mark edge visited and rotate the calipers + abVisited[iBIndex] = true; + if (++iBIndex == iQuantity) + { + iBIndex = 0; + } + } + break; + case F_RIGHT: + if (abVisited[iRIndex]) + { + bDone = true; + } + else + { + // compute dimensions of box with E[R] as an edge + kV = akEdge[iRIndex]; + kU = kV.Perp(); + UpdateBox(akHPoint[iLIndex],akHPoint[iRIndex], + akHPoint[iBIndex],akHPoint[iTIndex],kU,kV,fMinAreaDiv4, + kBox); + + // mark edge visited and rotate the calipers + abVisited[iRIndex] = true; + if (++iRIndex == iQuantity) + { + iRIndex = 0; + } + } + break; + case F_TOP: + if (abVisited[iTIndex]) + { + bDone = true; + } + else + { + // compute dimensions of box with E[T] as an edge + kU = -akEdge[iTIndex]; + kV = -kU.Perp(); + UpdateBox(akHPoint[iLIndex],akHPoint[iRIndex], + akHPoint[iBIndex],akHPoint[iTIndex],kU,kV,fMinAreaDiv4, + kBox); + + // mark edge visited and rotate the calipers + abVisited[iTIndex] = true; + if (++iTIndex == iQuantity) + { + iTIndex = 0; + } + } + break; + case F_LEFT: + if (abVisited[iLIndex]) + { + bDone = true; + } + else + { + // compute dimensions of box with E[L] as an edge + kV = -akEdge[iLIndex]; + kU = kV.Perp(); + UpdateBox(akHPoint[iLIndex],akHPoint[iRIndex], + akHPoint[iBIndex],akHPoint[iTIndex],kU,kV,fMinAreaDiv4, + kBox); + + // mark edge visited and rotate the calipers + abVisited[iLIndex] = true; + if (++iLIndex == iQuantity) + { + iLIndex = 0; + } + } + break; + case F_NONE: + // polygon is a rectangle + bDone = true; + break; + } + } + + WM4_DELETE[] abVisited; + WM4_DELETE[] akEdge; + if (!bIsConvexPolygon) + { + WM4_DELETE[] akHPoint; + } + + return kBox; +} +//---------------------------------------------------------------------------- +template +bool InBox (const Vector2& rkPoint, const Box2& rkBox) +{ + Vector2 kDiff = rkPoint - rkBox.Center; + for (int i = 0; i < 2; i++) + { + Real fCoeff = kDiff.Dot(rkBox.Axis[i]); + if (Math::FAbs(fCoeff) > rkBox.Extent[i]) + { + return false; + } + } + return true; +} +//---------------------------------------------------------------------------- +template +Box2 MergeBoxes (const Box2& rkBox0, const Box2& rkBox1) +{ + // construct a box that contains the input boxes + Box2 kBox; + + // The first guess at the box center. This value will be updated later + // after the input box vertices are projected onto axes determined by an + // average of box axes. + kBox.Center = ((Real)0.5)*(rkBox0.Center + rkBox1.Center); + + // The merged box axes are the averages of the input box axes. The + // axes of the second box are negated, if necessary, so they form acute + // angles with the axes of the first box. + if (rkBox0.Axis[0].Dot(rkBox1.Axis[0]) >= (Real)0.0) + { + kBox.Axis[0] = ((Real)0.5)*(rkBox0.Axis[0] + rkBox1.Axis[0]); + kBox.Axis[0].Normalize(); + } + else + { + kBox.Axis[0] = ((Real)0.5)*(rkBox0.Axis[0] - rkBox1.Axis[0]); + kBox.Axis[0].Normalize(); + } + kBox.Axis[1] = -kBox.Axis[0].Perp(); + + // Project the input box vertices onto the merged-box axes. Each axis + // D[i] containing the current center C has a minimum projected value + // pmin[i] and a maximum projected value pmax[i]. The corresponding end + // points on the axes are C+pmin[i]*D[i] and C+pmax[i]*D[i]. The point C + // is not necessarily the midpoint for any of the intervals. The actual + // box center will be adjusted from C to a point C' that is the midpoint + // of each interval, + // C' = C + sum_{i=0}^1 0.5*(pmin[i]+pmax[i])*D[i] + // The box extents are + // e[i] = 0.5*(pmax[i]-pmin[i]) + + int i, j; + Real fDot; + Vector2 akVertex[4], kDiff; + Vector2 kMin = Vector2::ZERO; + Vector2 kMax = Vector2::ZERO; + + rkBox0.ComputeVertices(akVertex); + for (i = 0; i < 4; i++) + { + kDiff = akVertex[i] - kBox.Center; + for (j = 0; j < 2; j++) + { + fDot = kDiff.Dot(kBox.Axis[j]); + if (fDot > kMax[j]) + { + kMax[j] = fDot; + } + else if ( fDot < kMin[j] ) + { + kMin[j] = fDot; + } + } + } + + rkBox1.ComputeVertices(akVertex); + for (i = 0; i < 4; i++) + { + kDiff = akVertex[i] - kBox.Center; + for (j = 0; j < 2; j++) + { + fDot = kDiff.Dot(kBox.Axis[j]); + if (fDot > kMax[j]) + { + kMax[j] = fDot; + } + else if (fDot < kMin[j]) + { + kMin[j] = fDot; + } + } + } + + // [kMin,kMax] is the axis-aligned box in the coordinate system of the + // merged box axes. Update the current box center to be the center of + // the new box. Compute the extents based on the new center. + for (j = 0; j < 2; j++) + { + kBox.Center += kBox.Axis[j]*(((Real)0.5)*(kMax[j]+kMin[j])); + kBox.Extent[j] = ((Real)0.5)*(kMax[j]-kMin[j]); + } + + return kBox; +} +//---------------------------------------------------------------------------- + +//---------------------------------------------------------------------------- +// explicit instantiation +//---------------------------------------------------------------------------- +template WM4_FOUNDATION_ITEM +Box2 ContAlignedBox (int, const Vector2*); + +template WM4_FOUNDATION_ITEM +Box2 ContOrientedBox (int, const Vector2*); + +template WM4_FOUNDATION_ITEM +Box2 ContMinBox (int, const Vector2*, float, + Query::Type, bool); + +template WM4_FOUNDATION_ITEM +bool InBox (const Vector2&, const Box2&); + +template WM4_FOUNDATION_ITEM +Box2 MergeBoxes (const Box2&, const Box2&); + +template WM4_FOUNDATION_ITEM +Box2 ContAlignedBox (int, const Vector2*); + +template WM4_FOUNDATION_ITEM +Box2 ContOrientedBox (int, const Vector2*); + +template WM4_FOUNDATION_ITEM +Box2 ContMinBox (int, const Vector2*, double, + Query::Type, bool); + +template WM4_FOUNDATION_ITEM +bool InBox (const Vector2&, const Box2&); + +template WM4_FOUNDATION_ITEM +Box2 MergeBoxes (const Box2&, const Box2&); +//---------------------------------------------------------------------------- +} diff --git a/src/Mod/Mesh/App/WildMagic4/Wm4ContBox2.h b/src/Mod/Mesh/App/WildMagic4/Wm4ContBox2.h new file mode 100644 index 0000000000..c5db59476d --- /dev/null +++ b/src/Mod/Mesh/App/WildMagic4/Wm4ContBox2.h @@ -0,0 +1,50 @@ +// Geometric Tools, LLC +// Copyright (c) 1998-2010 +// Distributed under the Boost Software License, Version 1.0. +// http://www.boost.org/LICENSE_1_0.txt +// http://www.geometrictools.com/License/Boost/LICENSE_1_0.txt +// +// File Version: 4.10.0 (2009/11/18) + +#ifndef WM4CONTBOX2_H +#define WM4CONTBOX2_H + +#include "Wm4FoundationLIB.h" +#include "Wm4Box2.h" +#include "Wm4Query.h" + +namespace Wm4 +{ + +// Compute the smallest axis-aligned bounding box of the points. +template WM4_FOUNDATION_ITEM +Box2 ContAlignedBox (int iQuantity, const Vector2* akPoint); + +// Compute an oriented bounding box of the points. The box center is the +// average of the points. The box axes are the eigenvectors of the covariance +// matrix. +template WM4_FOUNDATION_ITEM +Box2 ContOrientedBox (int iQuantity, const Vector2* akPoint); + +// Compute a minimum area oriented box containing the specified points. The +// algorithm uses the rotating calipers method. If the input points represent +// a counterclockwise-ordered polygon, set bIsConvexPolygon to 'true'. +template WM4_FOUNDATION_ITEM +Box2 ContMinBox (int iQuantity, const Vector2* akPoint, + Real fEpsilon, Query::Type eQueryType, bool bIsConvexPolygon); + +// Test for containment. Let X = C + y0*U0 + y1*U1 where C is the box center +// and U0 and U1 are the orthonormal axes of the box. X is in the box if +// |y_i| <= E_i for all i where E_i are the extents of the box. +template WM4_FOUNDATION_ITEM +bool InBox (const Vector2& rkPoint, const Box2& rkBox); + +// Construct an oriented box that contains two other oriented boxes. The +// result is not guaranteed to be the minimum volume box containing the +// input boxes. +template WM4_FOUNDATION_ITEM +Box2 MergeBoxes (const Box2& rkBox0, const Box2& rkBox1); + +} + +#endif diff --git a/src/Mod/Mesh/App/WildMagic4/Wm4ContBox3.cpp b/src/Mod/Mesh/App/WildMagic4/Wm4ContBox3.cpp new file mode 100644 index 0000000000..33649cd5a4 --- /dev/null +++ b/src/Mod/Mesh/App/WildMagic4/Wm4ContBox3.cpp @@ -0,0 +1,532 @@ +// Geometric Tools, LLC +// Copyright (c) 1998-2010 +// Distributed under the Boost Software License, Version 1.0. +// http://www.boost.org/LICENSE_1_0.txt +// http://www.geometrictools.com/License/Boost/LICENSE_1_0.txt +// +// File Version: 4.10.0 (2009/11/18) + +#include "Wm4FoundationPCH.h" +#include "Wm4ContBox3.h" +#include "Wm4ApprGaussPointsFit3.h" +#include "Wm4ContBox2.h" +#include "Wm4ConvexHull3.h" +#include "Wm4EdgeKey.h" +#include "Wm4Quaternion.h" + +namespace Wm4 +{ +//---------------------------------------------------------------------------- +template +Box3 ContAlignedBox (int iQuantity, const Vector3* akPoint) +{ + Vector3 kMin, kMax; + Vector3::ComputeExtremes(iQuantity,akPoint,kMin,kMax); + + Box3 kBox; + kBox.Center = ((Real)0.5)*(kMin + kMax); + kBox.Axis[0] = Vector3::UNIT_X; + kBox.Axis[1] = Vector3::UNIT_Y; + kBox.Axis[2] = Vector3::UNIT_Z; + Vector3 kHalfDiagonal = ((Real)0.5)*(kMax - kMin); + for (int i = 0; i < 3; i++) + { + kBox.Extent[i] = kHalfDiagonal[i]; + } + + return kBox; +} +//---------------------------------------------------------------------------- +template +Box3 ContOrientedBox (int iQuantity, const Vector3* akPoint) +{ + Box3 kBox = GaussPointsFit3(iQuantity,akPoint); + + // Let C be the box center and let U0, U1, and U2 be the box axes. Each + // input point is of the form X = C + y0*U0 + y1*U1 + y2*U2. The + // following code computes min(y0), max(y0), min(y1), max(y1), min(y2), + // and max(y2). The box center is then adjusted to be + // C' = C + 0.5*(min(y0)+max(y0))*U0 + 0.5*(min(y1)+max(y1))*U1 + + // 0.5*(min(y2)+max(y2))*U2 + + Vector3 kDiff = akPoint[0] - kBox.Center; + Vector3 kMin(kDiff.Dot(kBox.Axis[0]),kDiff.Dot(kBox.Axis[1]), + kDiff.Dot(kBox.Axis[2])); + Vector3 kMax = kMin; + for (int i = 1; i < iQuantity; i++) + { + kDiff = akPoint[i] - kBox.Center; + for (int j = 0; j < 3; j++) + { + Real fDot = kDiff.Dot(kBox.Axis[j]); + if (fDot < kMin[j]) + { + kMin[j] = fDot; + } + else if (fDot > kMax[j]) + { + kMax[j] = fDot; + } + } + } + + kBox.Center += + (((Real)0.5)*(kMin[0]+kMax[0]))*kBox.Axis[0] + + (((Real)0.5)*(kMin[1]+kMax[1]))*kBox.Axis[1] + + (((Real)0.5)*(kMin[2]+kMax[2]))*kBox.Axis[2]; + + kBox.Extent[0] = ((Real)0.5)*(kMax[0] - kMin[0]); + kBox.Extent[1] = ((Real)0.5)*(kMax[1] - kMin[1]); + kBox.Extent[2] = ((Real)0.5)*(kMax[2] - kMin[2]); + + return kBox; +} +//---------------------------------------------------------------------------- +template +Box3 ContMinBox (int iQuantity, const Vector3* akPoint, + Real fEpsilon, Query::Type eQueryType) +{ + Box3 kBox; + + // Get the convex hull of the points. + ConvexHull3 kHull(iQuantity,(Vector3*)akPoint,fEpsilon,false, + eQueryType); + int iHDim = kHull.GetDimension(); + int iHQuantity; + const int* aiHIndex; + + if (iHDim == 0) + { + kBox.Center = akPoint[0]; + kBox.Axis[0] = Vector3::UNIT_X; + kBox.Axis[1] = Vector3::UNIT_Y; + kBox.Axis[2] = Vector3::UNIT_Z; + kBox.Extent[0] = (Real)0.0; + kBox.Extent[1] = (Real)0.0; + kBox.Extent[2] = (Real)0.0; + return kBox; + } + + if (iHDim == 1) + { + ConvexHull1* pkHull1 = kHull.GetConvexHull1(); + aiHIndex = pkHull1->GetIndices(); + + kBox.Center = ((Real)0.5)*(akPoint[aiHIndex[0]]+akPoint[aiHIndex[1]]); + Vector3 kDiff = akPoint[aiHIndex[1]] - akPoint[aiHIndex[0]]; + kBox.Extent[0] = ((Real)0.5)*kDiff.Normalize(); + kBox.Extent[1] = (Real)0.0; + kBox.Extent[2] = (Real)0.0; + kBox.Axis[0] = kDiff; + Vector3::GenerateComplementBasis(kBox.Axis[1],kBox.Axis[2], + kBox.Axis[0]); + + WM4_DELETE pkHull1; + return kBox; + } + + int i, j; + Vector3 kOrigin, kDiff, kU, kV, kW; + Vector2* akPoint2; + Box2 kBox2; + + if (iHDim == 2) + { + // When ConvexHull3 reports that the point set is 2-dimensional, the + // caller is responsible for projecting the points onto a plane and + // calling ConvexHull2. ConvexHull3 does provide information about + // the plane of the points. In this application, we need only + // project the input points onto that plane and call ContMinBox in + // two dimensions. + + // Get a coordinate system relative to the plane of the points. + kOrigin = kHull.GetPlaneOrigin(); + kW = kHull.GetPlaneDirection(0).Cross(kHull.GetPlaneDirection(1)); + Vector3::GenerateComplementBasis(kU,kV,kW); + + // Project the input points onto the plane. + akPoint2 = WM4_NEW Vector2[iQuantity]; + for (i = 0; i < iQuantity; i++) + { + kDiff = akPoint[i] - kOrigin; + akPoint2[i].X() = kU.Dot(kDiff); + akPoint2[i].Y() = kV.Dot(kDiff); + } + + // Compute the minimum area box in 2D. + kBox2 = ContMinBox(iQuantity,akPoint2,fEpsilon,eQueryType, + false); + WM4_DELETE[] akPoint2; + + // Lift the values into 3D. + kBox.Center = kOrigin + kBox2.Center.X()*kU + kBox2.Center.Y()*kV; + kBox.Axis[0] = kBox2.Axis[0].X()*kU + kBox2.Axis[0].Y()*kV; + kBox.Axis[1] = kBox2.Axis[1].X()*kU + kBox2.Axis[1].Y()*kV; + kBox.Axis[2] = kW; + kBox.Extent[0] = kBox2.Extent[0]; + kBox.Extent[1] = kBox2.Extent[1]; + kBox.Extent[2] = (Real)0.0; + return kBox; + } + + iHQuantity = kHull.GetSimplexQuantity(); + aiHIndex = kHull.GetIndices(); + Real fVolume, fMinVolume = Math::MAX_REAL; + + // Create the unique set of hull vertices to minimize the time spent + // projecting vertices onto planes of the hull faces. + std::set kUniqueIndices; + for (i = 0; i < 3*iHQuantity; i++) + { + kUniqueIndices.insert(aiHIndex[i]); + } + + // Use the rotating calipers method on the projection of the hull onto + // the plane of each face. Also project the hull onto the normal line + // of each face. The minimum area box in the plane and the height on + // the line produce a containing box. If its volume is smaller than the + // current volume, this box is the new candidate for the minimum volume + // box. The unique edges are accumulated into a set for use by a later + // step in the algorithm. + const int* piIndex = aiHIndex; + Real fHeight, fMinHeight, fMaxHeight; + std::set kEdges; + akPoint2 = WM4_NEW Vector2[kUniqueIndices.size()]; + for (i = 0; i < iHQuantity; i++) + { + // get triangle + int iV0 = *piIndex++; + int iV1 = *piIndex++; + int iV2 = *piIndex++; + + // save the edges for later use + kEdges.insert(EdgeKey(iV0,iV1)); + kEdges.insert(EdgeKey(iV1,iV2)); + kEdges.insert(EdgeKey(iV2,iV0)); + + // get 3D coordinate system relative to plane of triangle + kOrigin = (akPoint[iV0] + akPoint[iV1] + akPoint[iV2])/(Real)3.0; + Vector3 kEdge1 = akPoint[iV1] - akPoint[iV0]; + Vector3 kEdge2 = akPoint[iV2] - akPoint[iV0]; + kW = kEdge2.UnitCross(kEdge1); // inner-pointing normal + if (kW == Vector3::ZERO) + { + // The triangle is needle-like, so skip it. + continue; + } + Vector3::GenerateComplementBasis(kU,kV,kW); + + // Project points onto plane of triangle, onto normal line of plane. + // TO DO. In theory, minHeight should be zero since W points to the + // interior of the hull. However, the snap rounding used in the 3D + // convex hull finder involves loss of precision, which in turn can + // cause a hull facet to have the wrong ordering (clockwise instead + // of counterclockwise when viewed from outside the hull). The + // height calculations here trap that problem (the incorrectly ordered + // face will not affect the minimum volume box calculations). + fMinHeight = (Real)0.0; + fMaxHeight = (Real)0.0; + j = 0; + std::set::const_iterator pkUI = kUniqueIndices.begin(); + while (pkUI != kUniqueIndices.end()) + { + int index = *pkUI++; + kDiff = akPoint[index] - kOrigin; + akPoint2[j].X() = kU.Dot(kDiff); + akPoint2[j].Y() = kV.Dot(kDiff); + fHeight = kW.Dot(kDiff); + if (fHeight > fMaxHeight) + { + fMaxHeight = fHeight; + } + else if (fHeight < fMinHeight) + { + fMinHeight = fHeight; + } + + j++; + } + if (-fMinHeight > fMaxHeight) + { + fMaxHeight = -fMinHeight; + } + + // compute minimum area box in 2D + kBox2 = ContMinBox((int)kUniqueIndices.size(),akPoint2,fEpsilon, + eQueryType,false); + + // update current minimum-volume box (if necessary) + fVolume = fMaxHeight*kBox2.Extent[0]*kBox2.Extent[1]; + if (fVolume < fMinVolume) + { + fMinVolume = fVolume; + + // lift the values into 3D + kBox.Extent[0] = kBox2.Extent[0]; + kBox.Extent[1] = kBox2.Extent[1]; + kBox.Extent[2] = ((Real)0.5)*fMaxHeight; + kBox.Axis[0] = kBox2.Axis[0].X()*kU + kBox2.Axis[0].Y()*kV; + kBox.Axis[1] = kBox2.Axis[1].X()*kU + kBox2.Axis[1].Y()*kV; + kBox.Axis[2] = kW; + kBox.Center = kOrigin + kBox2.Center.X()*kU + kBox2.Center.Y()*kV + + kBox.Extent[2]*kW; + } + } + + // The minimum-volume box can also be supported by three mutually + // orthogonal edges of the convex hull. For each triple of orthogonal + // edges, compute the minimum-volume box for that coordinate frame by + // projecting the points onto the axes of the frame. + std::set::const_iterator pkE2; + for (pkE2 = kEdges.begin(); pkE2 != kEdges.end(); pkE2++) + { + kW = akPoint[pkE2->V[1]] - akPoint[pkE2->V[0]]; + kW.Normalize(); + + std::set::const_iterator pkE1 = pkE2; + for (++pkE1; pkE1 != kEdges.end(); pkE1++) + { + kV = akPoint[pkE1->V[1]] - akPoint[pkE1->V[0]]; + kV.Normalize(); + Real fDot = kV.Dot(kW); + if (Math::FAbs(fDot) > Math::ZERO_TOLERANCE) + { + continue; + } + + std::set::const_iterator pkE0 = pkE1; + for (++pkE0; pkE0 != kEdges.end(); pkE0++) + { + kU = akPoint[pkE0->V[1]] - akPoint[pkE0->V[0]]; + kU.Normalize(); + fDot = kU.Dot(kV); + if (Math::FAbs(fDot) > Math::ZERO_TOLERANCE) + { + continue; + } + fDot = kU.Dot(kW); + if (Math::FAbs(fDot) > Math::ZERO_TOLERANCE) + { + continue; + } + + // The three edges are mutually orthogonal. Project the + // hull points onto the lines containing the edges. Use + // hull point zero as the origin. + Real fUMin = (Real)0.0, fUMax = (Real)0.0; + Real fVMin = (Real)0.0, fVMax = (Real)0.0; + Real fWMin = (Real)0.0, fWMax = (Real)0.0; + kOrigin = akPoint[aiHIndex[0]]; + std::set::const_iterator pkUI = kUniqueIndices.begin(); + while (pkUI != kUniqueIndices.end()) + { + int index = *pkUI++; + kDiff = akPoint[index] - kOrigin; + + Real fU = kU.Dot(kDiff); + if (fU < fUMin) + { + fUMin = fU; + } + else if (fU > fUMax) + { + fUMax = fU; + } + + Real fV = kV.Dot(kDiff); + if (fV < fVMin) + { + fVMin = fV; + } + else if (fV > fVMax) + { + fVMax = fV; + } + + Real fW = kW.Dot(kDiff); + if (fW < fWMin) + { + fWMin = fW; + } + else if (fW > fWMax) + { + fWMax = fW; + } + } + + Real fUExtent = ((Real)0.5)*(fUMax - fUMin); + Real fVExtent = ((Real)0.5)*(fVMax - fVMin); + Real fWExtent = ((Real)0.5)*(fWMax - fWMin); + + // update current minimum-volume box (if necessary) + fVolume = fUExtent*fVExtent*fWExtent; + if (fVolume < fMinVolume) + { + fMinVolume = fVolume; + + kBox.Extent[0] = fUExtent; + kBox.Extent[1] = fVExtent; + kBox.Extent[2] = fWExtent; + kBox.Axis[0] = kU; + kBox.Axis[1] = kV; + kBox.Axis[2] = kW; + kBox.Center = kOrigin + + ((Real)0.5)*(fUMin+fUMax)*kU + + ((Real)0.5)*(fVMin+fVMax)*kV + + ((Real)0.5)*(fWMin+fWMax)*kW; + } + } + } + } + + WM4_DELETE[] akPoint2; + return kBox; +} +//---------------------------------------------------------------------------- +template +bool InBox (const Vector3& rkPoint, const Box3& rkBox) +{ + Vector3 kDiff = rkPoint - rkBox.Center; + for (int i = 0; i < 3; i++) + { + Real fCoeff = kDiff.Dot(rkBox.Axis[i]); + if (Math::FAbs(fCoeff) > rkBox.Extent[i]) + { + return false; + } + } + return true; +} +//---------------------------------------------------------------------------- +template +Box3 MergeBoxes (const Box3& rkBox0, const Box3& rkBox1) +{ + // construct a box that contains the input boxes + Box3 kBox; + + // The first guess at the box center. This value will be updated later + // after the input box vertices are projected onto axes determined by an + // average of box axes. + kBox.Center = ((Real)0.5)*(rkBox0.Center + rkBox1.Center); + + // A box's axes, when viewed as the columns of a matrix, form a rotation + // matrix. The input box axes are converted to quaternions. The average + // quaternion is computed, then normalized to unit length. The result is + // the slerp of the two input quaternions with t-value of 1/2. The result + // is converted back to a rotation matrix and its columns are selected as + // the merged box axes. + Quaternion kQ0, kQ1; + kQ0.FromRotationMatrix(rkBox0.Axis); + kQ1.FromRotationMatrix(rkBox1.Axis); + if (kQ0.Dot(kQ1) < (Real)0.0) + { + kQ1 = -kQ1; + } + + Quaternion kQ = kQ0 + kQ1; + Real fInvLength = Math::InvSqrt(kQ.Dot(kQ)); + kQ = fInvLength*kQ; + kQ.ToRotationMatrix(kBox.Axis); + + // Project the input box vertices onto the merged-box axes. Each axis + // D[i] containing the current center C has a minimum projected value + // pmin[i] and a maximum projected value pmax[i]. The corresponding end + // points on the axes are C+pmin[i]*D[i] and C+pmax[i]*D[i]. The point C + // is not necessarily the midpoint for any of the intervals. The actual + // box center will be adjusted from C to a point C' that is the midpoint + // of each interval, + // C' = C + sum_{i=0}^2 0.5*(pmin[i]+pmax[i])*D[i] + // The box extents are + // e[i] = 0.5*(pmax[i]-pmin[i]) + + int i, j; + Real fDot; + Vector3 akVertex[8], kDiff; + Vector3 kMin = Vector3::ZERO; + Vector3 kMax = Vector3::ZERO; + + rkBox0.ComputeVertices(akVertex); + for (i = 0; i < 8; i++) + { + kDiff = akVertex[i] - kBox.Center; + for (j = 0; j < 3; j++) + { + fDot = kDiff.Dot(kBox.Axis[j]); + if (fDot > kMax[j]) + { + kMax[j] = fDot; + } + else if (fDot < kMin[j]) + { + kMin[j] = fDot; + } + } + } + + rkBox1.ComputeVertices(akVertex); + for (i = 0; i < 8; i++) + { + kDiff = akVertex[i] - kBox.Center; + for (j = 0; j < 3; j++) + { + fDot = kDiff.Dot(kBox.Axis[j]); + if (fDot > kMax[j]) + { + kMax[j] = fDot; + } + else if (fDot < kMin[j]) + { + kMin[j] = fDot; + } + } + } + + // [kMin,kMax] is the axis-aligned box in the coordinate system of the + // merged box axes. Update the current box center to be the center of + // the new box. Compute the extents based on the new center. + for (j = 0; j < 3; j++) + { + kBox.Center += (((Real)0.5)*(kMax[j]+kMin[j]))*kBox.Axis[j]; + kBox.Extent[j] = ((Real)0.5)*(kMax[j]-kMin[j]); + } + + return kBox; +} +//---------------------------------------------------------------------------- + +//---------------------------------------------------------------------------- +// explicit instantiation +//---------------------------------------------------------------------------- +template WM4_FOUNDATION_ITEM +Box3 ContAlignedBox (int, const Vector3*); + +template WM4_FOUNDATION_ITEM +Box3 ContOrientedBox (int, const Vector3*); + +template WM4_FOUNDATION_ITEM +Box3 ContMinBox (int, const Vector3*, float, + Query::Type); + +template WM4_FOUNDATION_ITEM +bool InBox (const Vector3&, const Box3&); + +template WM4_FOUNDATION_ITEM +Box3 MergeBoxes (const Box3&, const Box3&); + +template WM4_FOUNDATION_ITEM +Box3 ContAlignedBox (int, const Vector3*); + +template WM4_FOUNDATION_ITEM +Box3 ContOrientedBox (int, const Vector3*); + +template WM4_FOUNDATION_ITEM +Box3 ContMinBox (int, const Vector3*, double, + Query::Type); + +template WM4_FOUNDATION_ITEM +bool InBox (const Vector3&, const Box3&); + +template WM4_FOUNDATION_ITEM +Box3 MergeBoxes (const Box3&, const Box3&); +//---------------------------------------------------------------------------- +} diff --git a/src/Mod/Mesh/App/WildMagic4/Wm4ContBox3.h b/src/Mod/Mesh/App/WildMagic4/Wm4ContBox3.h new file mode 100644 index 0000000000..dad3b671f7 --- /dev/null +++ b/src/Mod/Mesh/App/WildMagic4/Wm4ContBox3.h @@ -0,0 +1,48 @@ +// Geometric Tools, LLC +// Copyright (c) 1998-2010 +// Distributed under the Boost Software License, Version 1.0. +// http://www.boost.org/LICENSE_1_0.txt +// http://www.geometrictools.com/License/Boost/LICENSE_1_0.txt +// +// File Version: 4.10.0 (2009/11/18) + +#ifndef WM4CONTBOX3_H +#define WM4CONTBOX3_H + +#include "Wm4FoundationLIB.h" +#include "Wm4Box3.h" +#include "Wm4Query.h" + +namespace Wm4 +{ + +// Compute the smallest axis-aligned bounding box of the points. +template WM4_FOUNDATION_ITEM +Box3 ContAlignedBox (int iQuantity, const Vector3* akPoint); + +// Compute an oriented bounding box of the points. The box center is the +// average of the points. The box axes are the eigenvectors of the covariance +// matrix. +template WM4_FOUNDATION_ITEM +Box3 ContOrientedBox (int iQuantity, const Vector3* akPoint); + +// Compute a minimum volume oriented box containing the specified points. +template WM4_FOUNDATION_ITEM +Box3 ContMinBox (int iQuantity, const Vector3* akPoint, + Real fEpsilon, Query::Type eQueryType); + +// Test for containment. Let X = C + y0*U0 + y1*U1 + y2*U2 where C is the +// box center and U0, U1, U2 are the orthonormal axes of the box. X is in +// the box if |y_i| <= E_i for all i where E_i are the extents of the box. +template WM4_FOUNDATION_ITEM +bool InBox (const Vector3& rkPoint, const Box3& rkBox); + +// Construct an oriented box that contains two other oriented boxes. The +// result is not guaranteed to be the minimum volume box containing the +// input boxes. +template WM4_FOUNDATION_ITEM +Box3 MergeBoxes (const Box3& rkBox0, const Box3& rkBox1); + +} + +#endif diff --git a/src/Mod/Mesh/App/WildMagic4/Wm4ConvexHull.cpp b/src/Mod/Mesh/App/WildMagic4/Wm4ConvexHull.cpp new file mode 100644 index 0000000000..40956b731f --- /dev/null +++ b/src/Mod/Mesh/App/WildMagic4/Wm4ConvexHull.cpp @@ -0,0 +1,146 @@ +// Geometric Tools, LLC +// Copyright (c) 1998-2010 +// Distributed under the Boost Software License, Version 1.0. +// http://www.boost.org/LICENSE_1_0.txt +// http://www.geometrictools.com/License/Boost/LICENSE_1_0.txt +// +// File Version: 4.10.0 (2009/11/18) + +#include "Wm4FoundationPCH.h" +#include "Wm4ConvexHull.h" + +namespace Wm4 +{ +//---------------------------------------------------------------------------- +template +ConvexHull::ConvexHull (int iVertexQuantity, Real fEpsilon, bool bOwner, + Query::Type eQueryType) +{ + assert(iVertexQuantity > 0 && fEpsilon >= (Real)0.0); + + m_eQueryType = eQueryType; + m_iVertexQuantity = iVertexQuantity; + m_iDimension = 0; + m_iSimplexQuantity = 0; + m_aiIndex = 0; + m_fEpsilon = fEpsilon; + m_bOwner = bOwner; +} +//---------------------------------------------------------------------------- +template +ConvexHull::~ConvexHull () +{ + WM4_DELETE[] m_aiIndex; +} +//---------------------------------------------------------------------------- +template +int ConvexHull::GetQueryType () const +{ + return m_eQueryType; +} +//---------------------------------------------------------------------------- +template +int ConvexHull::GetVertexQuantity () const +{ + return m_iVertexQuantity; +} +//---------------------------------------------------------------------------- +template +Real ConvexHull::GetEpsilon () const +{ + return m_fEpsilon; +} +//---------------------------------------------------------------------------- +template +bool ConvexHull::GetOwner () const +{ + return m_bOwner; +} +//---------------------------------------------------------------------------- +template +int ConvexHull::GetDimension () const +{ + return m_iDimension; +} +//---------------------------------------------------------------------------- +template +int ConvexHull::GetSimplexQuantity () const +{ + return m_iSimplexQuantity; +} +//---------------------------------------------------------------------------- +template +const int* ConvexHull::GetIndices () const +{ + return m_aiIndex; +} +//---------------------------------------------------------------------------- +template +bool ConvexHull::Load (FILE* pkIFile) +{ + WM4_DELETE[] m_aiIndex; + + // fixed-size members + int iQueryType; + System::Read4le(pkIFile,1,&iQueryType); + m_eQueryType = (Query::Type)iQueryType; + System::Read4le(pkIFile,1,&m_iVertexQuantity); + System::Read4le(pkIFile,1,&m_iDimension); + System::Read4le(pkIFile,1,&m_iSimplexQuantity); + System::Read4le(pkIFile,1,&m_fEpsilon); + + // variable-size members + int iIQuantity; + System::Read4le(pkIFile,1,&iIQuantity); + if (1 <= m_iDimension && m_iDimension <= 3) + { + assert(iIQuantity == (m_iDimension+1)*m_iSimplexQuantity); + m_aiIndex = WM4_NEW int[iIQuantity]; + System::Read4le(pkIFile,iIQuantity,m_aiIndex); + return true; + } + + m_aiIndex = 0; + return m_iDimension == 0; +} +//---------------------------------------------------------------------------- +template +bool ConvexHull::Save (FILE* pkOFile) const +{ + // fixed-size members + int iQueryType = (int)m_eQueryType; + System::Write4le(pkOFile,1,&iQueryType); + System::Write4le(pkOFile,1,&m_iVertexQuantity); + System::Write4le(pkOFile,1,&m_iDimension); + System::Write4le(pkOFile,1,&m_iSimplexQuantity); + System::Write4le(pkOFile,1,&m_fEpsilon); + + // The member m_bOwner is not streamed because on a Load call, this + // object will allocate the vertices and own this memory. + + // variable-size members + int iIQuantity; + if (1 <= m_iDimension && m_iDimension <= 3) + { + iIQuantity = (m_iDimension+1)*m_iSimplexQuantity; + System::Write4le(pkOFile,1,&iIQuantity); + System::Write4le(pkOFile,iIQuantity,m_aiIndex); + return true; + } + + iIQuantity = 0; + System::Write4le(pkOFile,1,&iIQuantity); + return m_iDimension == 0; +} +//---------------------------------------------------------------------------- + +//---------------------------------------------------------------------------- +// explicit instantiation +//---------------------------------------------------------------------------- +template WM4_FOUNDATION_ITEM +class ConvexHull; + +template WM4_FOUNDATION_ITEM +class ConvexHull; +//---------------------------------------------------------------------------- +} diff --git a/src/Mod/Mesh/App/WildMagic4/Wm4ConvexHull.h b/src/Mod/Mesh/App/WildMagic4/Wm4ConvexHull.h new file mode 100644 index 0000000000..629735345b --- /dev/null +++ b/src/Mod/Mesh/App/WildMagic4/Wm4ConvexHull.h @@ -0,0 +1,100 @@ +// Geometric Tools, LLC +// Copyright (c) 1998-2010 +// Distributed under the Boost Software License, Version 1.0. +// http://www.boost.org/LICENSE_1_0.txt +// http://www.geometrictools.com/License/Boost/LICENSE_1_0.txt +// +// File Version: 4.10.0 (2009/11/18) + +#ifndef WM4CONVEXHULL_H +#define WM4CONVEXHULL_H + +#include "Wm4FoundationLIB.h" +#include "Wm4System.h" +#include "Wm4Query.h" + +namespace Wm4 +{ + +template +class WM4_FOUNDATION_ITEM ConvexHull +{ +public: + // Abstract base class. + virtual ~ConvexHull (); + + // Member accessors. For notational purposes in this class documentation, + // The number of vertices is VQ and the vertex array is V. + int GetQueryType () const; + int GetVertexQuantity () const; + Real GetEpsilon () const; + bool GetOwner () const; + + // The dimension of the result, call it d. If n is the dimension of the + // space of the input points, then 0 <= d <= n. + int GetDimension () const; + + // The interpretations of the return values of these functions depends on + // the dimension. Generally, SQ = GetSimplexQuantity() is the number of + // simplices in the mesh. The array I is returned by GetIndices(). + int GetSimplexQuantity () const; + const int* GetIndices () const; + + // Dimension d = 0. + // SQ = 1 + // I = null (use index zero for vertices) + + // Dimension d = 1. + // SQ = 2 + // I = array of two indices + // The segment has end points + // vertex[0] = V[I[2*i+0]] + // vertex[1] = V[I[2*i+1]] + + // Dimension d = 2. + // SQ = number of convex polygon edges + // I = array of into V that represent the convex polygon edges + // (SQ total elements) + // The i-th edge has vertices + // vertex[0] = V[I[2*i+0]] + // vertex[1] = V[I[2*i+1]] + + // Dimension d = 3. + // SQ = number of convex polyhedron triangular faces + // I = array of 3-tuples of indices into V that represent the + // triangles (3*SQ total elements) + // The i-th face has vertices + // vertex[0] = V[I[3*i+0]] + // vertex[1] = V[I[3*i+1]] + // vertex[2] = V[I[3*i+2]] + +protected: + // Abstract base class. The number of vertices to be processed is + // iVQuantity. The value of fEpsilon is a tolerance used for determining + // the intrinsic dimension of the input set of vertices. Ownership of the + // input points to the constructors for the derived classes may be + // transferred to this class. If you want the input vertices to be + // deleted by this class, set bOwner to 'true'; otherwise, you own the + // array and must delete it yourself. + ConvexHull (int iVertexQuantity, Real fEpsilon, bool bOwner, + Query::Type eQueryType); + + // Support for streaming to/from disk. + bool Load (FILE* pkIFile); + bool Save (FILE* pkOFile) const; + + Query::Type m_eQueryType; + int m_iVertexQuantity; + int m_iDimension; + int m_iSimplexQuantity; + int* m_aiIndex; + Real m_fEpsilon; + bool m_bOwner; +}; + +typedef ConvexHull ConvexHullf; +typedef ConvexHull ConvexHulld; + +} + +#endif diff --git a/src/Mod/Mesh/App/WildMagic4/Wm4ConvexHull1.cpp b/src/Mod/Mesh/App/WildMagic4/Wm4ConvexHull1.cpp new file mode 100644 index 0000000000..f1039ad1f5 --- /dev/null +++ b/src/Mod/Mesh/App/WildMagic4/Wm4ConvexHull1.cpp @@ -0,0 +1,138 @@ +// Geometric Tools, LLC +// Copyright (c) 1998-2010 +// Distributed under the Boost Software License, Version 1.0. +// http://www.boost.org/LICENSE_1_0.txt +// http://www.geometrictools.com/License/Boost/LICENSE_1_0.txt +// +// File Version: 4.10.0 (2009/11/18) + +#include "Wm4FoundationPCH.h" +#include "Wm4ConvexHull1.h" + +namespace Wm4 +{ +//---------------------------------------------------------------------------- +template +ConvexHull1::ConvexHull1 (int iVertexQuantity, Real* afVertex, + Real fEpsilon, bool bOwner, Query::Type eQueryType) + : + ConvexHull(iVertexQuantity,fEpsilon,bOwner,eQueryType) +{ + assert(afVertex); + m_afVertex = afVertex; + + std::vector kArray(m_iVertexQuantity); + int i; + for (i = 0; i < m_iVertexQuantity; i++) + { + kArray[i].Value = m_afVertex[i]; + kArray[i].Index = i; + } + std::sort(kArray.begin(),kArray.end()); + + Real fRange = kArray[m_iVertexQuantity-1].Value - kArray[0].Value; + if (fRange >= m_fEpsilon) + { + m_iDimension = 1; + m_iSimplexQuantity = 2; + m_aiIndex = WM4_NEW int[2]; + m_aiIndex[0] = kArray[0].Index; + m_aiIndex[1] = kArray[m_iVertexQuantity-1].Index; + } +} +//---------------------------------------------------------------------------- +template +ConvexHull1::~ConvexHull1 () +{ + if (m_bOwner) + { + WM4_DELETE[] m_afVertex; + } +} +//---------------------------------------------------------------------------- +template +const Real* ConvexHull1::GetVertices () const +{ + return m_afVertex; +} +//---------------------------------------------------------------------------- +template +ConvexHull1::ConvexHull1 (const char* acFilename) + : + ConvexHull(0,(Real)0.0,false,Query::QT_REAL) +{ + m_afVertex = 0; + bool bLoaded = Load(acFilename); + assert(bLoaded); + (void)bLoaded; // avoid warning in Release build +} +//---------------------------------------------------------------------------- +template +bool ConvexHull1::Load (const char* acFilename) +{ + FILE* pkIFile = System::Fopen(acFilename,"rb"); + if (!pkIFile) + { + return false; + } + + ConvexHull::Load(pkIFile); + + if (m_bOwner) + { + WM4_DELETE[] m_afVertex; + } + + m_bOwner = true; + m_afVertex = WM4_NEW Real[m_iVertexQuantity]; + + size_t uiSize = sizeof(Real); + if (uiSize == 4) + { + System::Read4le(pkIFile,m_iVertexQuantity,m_afVertex); + } + else // uiSize == 8 + { + System::Read8le(pkIFile,m_iVertexQuantity,m_afVertex); + } + + System::Fclose(pkIFile); + return true; +} +//---------------------------------------------------------------------------- +template +bool ConvexHull1::Save (const char* acFilename) const +{ + FILE* pkOFile = System::Fopen(acFilename,"wb"); + if (!pkOFile) + { + return false; + } + + ConvexHull::Save(pkOFile); + + size_t uiSize = sizeof(Real); + if (uiSize == 4) + { + System::Write4le(pkOFile,m_iVertexQuantity,m_afVertex); + } + else // uiSize == 8 + { + System::Write8le(pkOFile,m_iVertexQuantity,m_afVertex); + } + + System::Fclose(pkOFile); + return true; +} +//---------------------------------------------------------------------------- + +//---------------------------------------------------------------------------- +// explicit instantiation +//---------------------------------------------------------------------------- +template WM4_FOUNDATION_ITEM +class ConvexHull1; + +template WM4_FOUNDATION_ITEM +class ConvexHull1; +//---------------------------------------------------------------------------- +} diff --git a/src/Mod/Mesh/App/WildMagic4/Wm4ConvexHull1.h b/src/Mod/Mesh/App/WildMagic4/Wm4ConvexHull1.h new file mode 100644 index 0000000000..ff35c91ded --- /dev/null +++ b/src/Mod/Mesh/App/WildMagic4/Wm4ConvexHull1.h @@ -0,0 +1,73 @@ +// Geometric Tools, LLC +// Copyright (c) 1998-2010 +// Distributed under the Boost Software License, Version 1.0. +// http://www.boost.org/LICENSE_1_0.txt +// http://www.geometrictools.com/License/Boost/LICENSE_1_0.txt +// +// File Version: 4.10.0 (2009/11/18) + +#ifndef WM4CONVEXHULL1_H +#define WM4CONVEXHULL1_H + +// A fancy class to compute the minimum and maximum of a collection of +// real-valued numbers, but this provides some convenience for ConvexHull2 and +// ConvexHull3 when the input point set has intrinsic dimension smaller than +// the containing space. The interface of ConvexHull1 is also the model for +// those of ConvexHull2 and ConvexHull3. + +#include "Wm4FoundationLIB.h" +#include "Wm4ConvexHull.h" + +namespace Wm4 +{ + +template +class WM4_FOUNDATION_ITEM ConvexHull1 : public ConvexHull +{ +public: + // The input to the constructor is the array of vertices you want to sort. + // If you want ConvexHull1 to delete the array during destruction, set + // bOwner to 'true'. Otherwise, you own the array and must delete it + // yourself. TO DO: The computation type is currently ignored by this + // class. Add support for the various types later. + ConvexHull1 (int iVertexQuantity, Real* afVertex, Real fEpsilon, + bool bOwner, Query::Type eQueryType); + virtual ~ConvexHull1 (); + + // The input vertex array. + const Real* GetVertices () const; + + // Support for streaming to/from disk. + ConvexHull1 (const char* acFilename); + bool Load (const char* acFilename); + bool Save (const char* acFilename) const; + +private: + using ConvexHull::m_iVertexQuantity; + using ConvexHull::m_iDimension; + using ConvexHull::m_iSimplexQuantity; + using ConvexHull::m_aiIndex; + using ConvexHull::m_fEpsilon; + using ConvexHull::m_bOwner; + + Real* m_afVertex; + + class WM4_FOUNDATION_ITEM SortedVertex + { + public: + Real Value; + int Index; + + bool operator< (const SortedVertex& rkProj) const + { + return Value < rkProj.Value; + } + }; +}; + +typedef ConvexHull1 ConvexHull1f; +typedef ConvexHull1 ConvexHull1d; + +} + +#endif diff --git a/src/Mod/Mesh/App/WildMagic4/Wm4ConvexHull2.cpp b/src/Mod/Mesh/App/WildMagic4/Wm4ConvexHull2.cpp new file mode 100644 index 0000000000..9198d4b6f4 --- /dev/null +++ b/src/Mod/Mesh/App/WildMagic4/Wm4ConvexHull2.cpp @@ -0,0 +1,488 @@ +// Geometric Tools, LLC +// Copyright (c) 1998-2010 +// Distributed under the Boost Software License, Version 1.0. +// http://www.boost.org/LICENSE_1_0.txt +// http://www.geometrictools.com/License/Boost/LICENSE_1_0.txt +// +// File Version: 4.10.0 (2009/11/18) + +#include "Wm4FoundationPCH.h" +#include "Wm4ConvexHull2.h" +#include "Wm4Mapper2.h" +#include "Wm4Query2Filtered.h" +#include "Wm4Query2Int64.h" +#include "Wm4Query2TInteger.h" +#include "Wm4Query2TRational.h" + +namespace Wm4 +{ + +//---------------------------------------------------------------------------- +template +ConvexHull2::ConvexHull2 (int iVertexQuantity, Vector2* akVertex, + Real fEpsilon, bool bOwner, Query::Type eQueryType) + : + ConvexHull(iVertexQuantity,fEpsilon,bOwner,eQueryType), + m_kLineOrigin(Vector2::ZERO), + m_kLineDirection(Vector2::ZERO) +{ + assert(akVertex); + m_akVertex = akVertex; + m_akSVertex = 0; + m_pkQuery = 0; + + Mapper2 kMapper(m_iVertexQuantity,m_akVertex,m_fEpsilon); + if (kMapper.GetDimension() == 0) + { + // The values of m_iDimension, m_aiIndex, and m_aiAdjacent were + // already initialized by the ConvexHull base class. + return; + } + + if (kMapper.GetDimension() == 1) + { + // The set is (nearly) collinear. The caller is responsible for + // creating a ConvexHull1 object. + m_iDimension = 1; + m_kLineOrigin = kMapper.GetOrigin(); + m_kLineDirection = kMapper.GetDirection(0); + return; + } + + m_iDimension = 2; + + int i0 = kMapper.GetExtremeIndex(0); + int i1 = kMapper.GetExtremeIndex(1); + int i2 = kMapper.GetExtremeIndex(2); + + m_akSVertex = WM4_NEW Vector2[m_iVertexQuantity]; + int i; + + if (eQueryType != Query::QT_RATIONAL && eQueryType != Query::QT_FILTERED) + { + // Transform the vertices to the square [0,1]^2. + Vector2 kMin = kMapper.GetMin(); + Real fScale = ((Real)1.0)/kMapper.GetMaxRange(); + for (i = 0; i < m_iVertexQuantity; i++) + { + m_akSVertex[i] = (m_akVertex[i] - kMin)*fScale; + } + + Real fExpand; + if (eQueryType == Query::QT_INT64) + { + // Scale the vertices to the square [0,2^{20}]^2 to allow use of + // 64-bit integers. + fExpand = (Real)(1 << 20); + m_pkQuery = WM4_NEW Query2Int64(m_iVertexQuantity, + m_akSVertex); + } + else if (eQueryType == Query::QT_INTEGER) + { + // Scale the vertices to the square [0,2^{24}]^2 to allow use of + // TInteger. + fExpand = (Real)(1 << 24); + m_pkQuery = WM4_NEW Query2TInteger(m_iVertexQuantity, + m_akSVertex); + } + else // eQueryType == Query::QT_REAL + { + // No scaling for floating point. + fExpand = (Real)1.0; + m_pkQuery = WM4_NEW Query2(m_iVertexQuantity,m_akSVertex); + } + + for (i = 0; i < m_iVertexQuantity; i++) + { + m_akSVertex[i] *= fExpand; + } + } + else + { + // No transformation needed for exact rational arithmetic or filtered + // predicates. + size_t uiSize = m_iVertexQuantity*sizeof(Vector2); + System::Memcpy(m_akSVertex,uiSize,m_akVertex,uiSize); + + if (eQueryType == Query::QT_RATIONAL) + { + m_pkQuery = WM4_NEW Query2TRational(m_iVertexQuantity, + m_akSVertex); + } + else // eQueryType == Query::QT_FILTERED + { + m_pkQuery = WM4_NEW Query2Filtered(m_iVertexQuantity, + m_akSVertex,m_fEpsilon); + } + } + + Edge* pkE0; + Edge* pkE1; + Edge* pkE2; + + if (kMapper.GetExtremeCCW()) + { + pkE0 = WM4_NEW Edge(i0,i1); + pkE1 = WM4_NEW Edge(i1,i2); + pkE2 = WM4_NEW Edge(i2,i0); + } + else + { + pkE0 = WM4_NEW Edge(i0,i2); + pkE1 = WM4_NEW Edge(i2,i1); + pkE2 = WM4_NEW Edge(i1,i0); + } + + pkE0->Insert(pkE2,pkE1); + pkE1->Insert(pkE0,pkE2); + pkE2->Insert(pkE1,pkE0); + + Edge* pkHull = pkE0; + for (i = 0; i < m_iVertexQuantity; i++) + { + if (!Update(pkHull,i)) + { + pkHull->DeleteAll(); + return; + } + } + + pkHull->GetIndices(m_iSimplexQuantity,m_aiIndex); + pkHull->DeleteAll(); +} +//---------------------------------------------------------------------------- +template +ConvexHull2::~ConvexHull2 () +{ + if (m_bOwner) + { + WM4_DELETE[] m_akVertex; + } + WM4_DELETE[] m_akSVertex; + WM4_DELETE m_pkQuery; +} +//---------------------------------------------------------------------------- +template +const Vector2& ConvexHull2::GetLineOrigin () const +{ + return m_kLineOrigin; +} +//---------------------------------------------------------------------------- +template +const Vector2& ConvexHull2::GetLineDirection () const +{ + return m_kLineDirection; +} +//---------------------------------------------------------------------------- +template +ConvexHull1* ConvexHull2::GetConvexHull1 () const +{ + assert(m_iDimension == 1); + if (m_iDimension != 1) + { + return 0; + } + + Real* afProjection = WM4_NEW Real[m_iVertexQuantity]; + for (int i = 0; i < m_iVertexQuantity; i++) + { + Vector2 kDiff = m_akVertex[i] - m_kLineOrigin; + afProjection[i] = m_kLineDirection.Dot(kDiff); + } + + return WM4_NEW ConvexHull1(m_iVertexQuantity,afProjection, + m_fEpsilon,true,m_eQueryType); +} +//---------------------------------------------------------------------------- +template +bool ConvexHull2::Update (Edge*& rpkHull, int i) +{ + // Locate an edge visible to the input point (if possible). + Edge* pkVisible = 0; + Edge* pkCurrent = rpkHull; + do + { + if (pkCurrent->GetSign(i,m_pkQuery) > 0) + { + pkVisible = pkCurrent; + break; + } + + pkCurrent = pkCurrent->A[1]; + } + while (pkCurrent != rpkHull); + + if (!pkVisible) + { + // The point is inside the current hull; nothing to do. + return true; + } + + // Remove the visible edges. + Edge* pkAdj0 = pkVisible->A[0]; + assert(pkAdj0); + if (!pkAdj0) + { + return false; + } + + Edge* pkAdj1 = pkVisible->A[1]; + assert(pkAdj1); + if (!pkAdj1) + { + return false; + } + + pkVisible->DeleteSelf(); + + while (pkAdj0->GetSign(i,m_pkQuery) > 0) + { + rpkHull = pkAdj0; + pkAdj0 = pkAdj0->A[0]; + assert(pkAdj0); + if (!pkAdj0) + { + return false; + } + + pkAdj0->A[1]->DeleteSelf(); + } + + while (pkAdj1->GetSign(i,m_pkQuery) > 0) + { + rpkHull = pkAdj1; + pkAdj1 = pkAdj1->A[1]; + assert(pkAdj1); + if (!pkAdj1) + { + return false; + } + + pkAdj1->A[0]->DeleteSelf(); + } + + // Insert the new edges formed by the input point and the end points of + // the polyline of invisible edges. + Edge* pkEdge0 = WM4_NEW Edge(pkAdj0->V[1],i); + Edge* pkEdge1 = WM4_NEW Edge(i,pkAdj1->V[0]); + pkEdge0->Insert(pkAdj0,pkEdge1); + pkEdge1->Insert(pkEdge0,pkAdj1); + rpkHull = pkEdge0; + + return true; +} +//---------------------------------------------------------------------------- +template +ConvexHull2::ConvexHull2 (const char* acFilename) + : + ConvexHull(0,(Real)0.0,false,Query::QT_REAL) +{ + m_akVertex = 0; + m_akSVertex = 0; + m_pkQuery = 0; + bool bLoaded = Load(acFilename); + assert(bLoaded); + (void)bLoaded; // avoid warning in Release build +} +//---------------------------------------------------------------------------- +template +bool ConvexHull2::Load (const char* acFilename) +{ + FILE* pkIFile = System::Fopen(acFilename,"rb"); + if (!pkIFile) + { + return false; + } + + ConvexHull::Load(pkIFile); + + WM4_DELETE m_pkQuery; + WM4_DELETE[] m_akSVertex; + if (m_bOwner) + { + WM4_DELETE[] m_akVertex; + } + + m_bOwner = true; + m_akVertex = WM4_NEW Vector2[m_iVertexQuantity]; + m_akSVertex = WM4_NEW Vector2[m_iVertexQuantity]; + + size_t uiSize = sizeof(Real); + int iVQ = 2*m_iVertexQuantity; + if (uiSize == 4) + { + System::Read4le(pkIFile,iVQ,m_akVertex); + System::Read4le(pkIFile,iVQ,m_akSVertex); + System::Read4le(pkIFile,2,(Real*)m_kLineOrigin); + System::Read4le(pkIFile,2,(Real*)m_kLineDirection); + } + else // iSize == 8 + { + System::Read8le(pkIFile,iVQ,m_akVertex); + System::Read8le(pkIFile,iVQ,m_akSVertex); + System::Read8le(pkIFile,2,(Real*)m_kLineOrigin); + System::Read8le(pkIFile,2,(Real*)m_kLineDirection); + } + + System::Fclose(pkIFile); + + switch (m_eQueryType) + { + case Query::QT_INT64: + m_pkQuery = WM4_NEW Query2Int64(m_iVertexQuantity,m_akSVertex); + break; + case Query::QT_INTEGER: + m_pkQuery = WM4_NEW Query2TInteger(m_iVertexQuantity, + m_akSVertex); + break; + case Query::QT_RATIONAL: + m_pkQuery = WM4_NEW Query2TRational(m_iVertexQuantity, + m_akSVertex); + break; + case Query::QT_REAL: + m_pkQuery = WM4_NEW Query2(m_iVertexQuantity,m_akSVertex); + break; + case Query::QT_FILTERED: + m_pkQuery = WM4_NEW Query2Filtered(m_iVertexQuantity, + m_akSVertex,m_fEpsilon); + break; + } + + return true; +} +//---------------------------------------------------------------------------- +template +bool ConvexHull2::Save (const char* acFilename) const +{ + FILE* pkOFile = System::Fopen(acFilename,"wb"); + if (!pkOFile) + { + return false; + } + + ConvexHull::Save(pkOFile); + + size_t uiSize = sizeof(Real); + int iVQ = 2*m_iVertexQuantity; + if (uiSize == 4) + { + System::Write4le(pkOFile,iVQ,m_akVertex); + System::Write4le(pkOFile,iVQ,m_akSVertex); + System::Write4le(pkOFile,2,(const Real*)m_kLineOrigin); + System::Write4le(pkOFile,2,(const Real*)m_kLineDirection); + } + else // iSize == 8 + { + System::Write8le(pkOFile,iVQ,m_akVertex); + System::Write8le(pkOFile,iVQ,m_akSVertex); + System::Write8le(pkOFile,2,(const Real*)m_kLineOrigin); + System::Write8le(pkOFile,2,(const Real*)m_kLineDirection); + } + + System::Fclose(pkOFile); + return true; +} +//---------------------------------------------------------------------------- + +//---------------------------------------------------------------------------- +// ConvexHull2::Edge +//---------------------------------------------------------------------------- +template +ConvexHull2::Edge::Edge (int iV0, int iV1) +{ + V[0] = iV0; + V[1] = iV1; + A[0] = 0; + A[1] = 0; + Sign = 0; + Time = -1; +} +//---------------------------------------------------------------------------- +template +int ConvexHull2::Edge::GetSign (int i, const Query2* pkQuery) +{ + if (i != Time) + { + Time = i; + Sign = pkQuery->ToLine(i,V[0],V[1]); + } + + return Sign; +} +//---------------------------------------------------------------------------- +template +void ConvexHull2::Edge::Insert (Edge* pkAdj0, Edge* pkAdj1) +{ + pkAdj0->A[1] = this; + pkAdj1->A[0] = this; + A[0] = pkAdj0; + A[1] = pkAdj1; +} +//---------------------------------------------------------------------------- +template +void ConvexHull2::Edge::DeleteSelf () +{ + if (A[0]) + { + A[0]->A[1] = 0; + } + + if (A[1]) + { + A[1]->A[0] = 0; + } + + WM4_DELETE this; +} +//---------------------------------------------------------------------------- +template +void ConvexHull2::Edge::DeleteAll () +{ + Edge* pkAdj = A[1]; + while (pkAdj && pkAdj != this) + { + Edge* pkSave = pkAdj->A[1]; + WM4_DELETE pkAdj; + pkAdj = pkSave; + } + + assert(pkAdj == this); + WM4_DELETE this; +} +//---------------------------------------------------------------------------- +template +void ConvexHull2::Edge::GetIndices (int& riHQuantity, int*& raiHIndex) +{ + // Count the number of edge vertices and allocate the index array. + riHQuantity = 0; + Edge* pkCurrent = this; + do + { + riHQuantity++; + pkCurrent = pkCurrent->A[1]; + } + while (pkCurrent != this); + raiHIndex = WM4_NEW int[riHQuantity]; + + // Fill the index array. + riHQuantity = 0; + pkCurrent = this; + do + { + raiHIndex[riHQuantity++] = pkCurrent->V[0]; + pkCurrent = pkCurrent->A[1]; + } + while (pkCurrent != this); +} +//---------------------------------------------------------------------------- + +//---------------------------------------------------------------------------- +// explicit instantiation +//---------------------------------------------------------------------------- +template WM4_FOUNDATION_ITEM +class ConvexHull2; + +template WM4_FOUNDATION_ITEM +class ConvexHull2; +//---------------------------------------------------------------------------- +} diff --git a/src/Mod/Mesh/App/WildMagic4/Wm4ConvexHull2.h b/src/Mod/Mesh/App/WildMagic4/Wm4ConvexHull2.h new file mode 100644 index 0000000000..46d395e02a --- /dev/null +++ b/src/Mod/Mesh/App/WildMagic4/Wm4ConvexHull2.h @@ -0,0 +1,96 @@ +// Geometric Tools, LLC +// Copyright (c) 1998-2010 +// Distributed under the Boost Software License, Version 1.0. +// http://www.boost.org/LICENSE_1_0.txt +// http://www.geometrictools.com/License/Boost/LICENSE_1_0.txt +// +// File Version: 4.10.0 (2009/11/18) + +#ifndef WM4CONVEXHULL2_H +#define WM4CONVEXHULL2_H + +#include "Wm4FoundationLIB.h" +#include "Wm4ConvexHull1.h" +#include "Wm4Query2.h" + +namespace Wm4 +{ + +template +class WM4_FOUNDATION_ITEM ConvexHull2 : public ConvexHull +{ +public: + // The input to the constructor is the array of vertices whose convex hull + // is required. If you want ConvexHull2 to delete the vertices during + // destruction, set bOwner to 'true'. Otherwise, you own the vertices and + // must delete them yourself. + // + // You have a choice of speed versus accuracy. The fastest choice is + // Query::QT_INT64, but it gives up a lot of precision, scaling the points + // to [0,2^{20}]^3. The choice Query::QT_INTEGER gives up less precision, + // scaling the points to [0,2^{24}]^3. The choice Query::QT_RATIONAL uses + // exact arithmetic, but is the slowest choice. The choice Query::QT_REAL + // uses floating-point arithmetic, but is not robust in all cases. + + ConvexHull2 (int iVertexQuantity, Vector2* akVertex, Real fEpsilon, + bool bOwner, Query::Type eQueryType); + virtual ~ConvexHull2 (); + + // If GetDimension() returns 1, then the points lie on a line. You must + // create a ConvexHull1 object using the function provided. + const Vector2& GetLineOrigin () const; + const Vector2& GetLineDirection () const; + ConvexHull1* GetConvexHull1 () const; + +private: + using ConvexHull::m_eQueryType; + using ConvexHull::m_iVertexQuantity; + using ConvexHull::m_iDimension; + using ConvexHull::m_iSimplexQuantity; + using ConvexHull::m_aiIndex; + using ConvexHull::m_fEpsilon; + using ConvexHull::m_bOwner; + + class WM4_FOUNDATION_ITEM Edge + { + public: + Edge (int iV0, int iV1); + + int GetSign (int i, const Query2* pkQuery); + + void Insert (Edge* pkAdj0, Edge* pkAdj1); + void DeleteSelf (); + void DeleteAll (); + + void GetIndices (int& riHQuantity, int*& raiHIndex); + + int V[2]; + Edge* A[2]; + int Sign; + int Time; + }; + + // Support for streaming to/from disk. + ConvexHull2 (const char* acFilename); + bool Load (const char* acFilename); + bool Save (const char* acFilename) const; + + bool Update (Edge*& rpkHull, int i); + + // The input points. + Vector2* m_akVertex; + + // Support for robust queries. + Vector2* m_akSVertex; + Query2* m_pkQuery; + + // The line of containment if the dimension is 1. + Vector2 m_kLineOrigin, m_kLineDirection; +}; + +typedef ConvexHull2 ConvexHull2f; +typedef ConvexHull2 ConvexHull2d; + +} + +#endif diff --git a/src/Mod/Mesh/App/WildMagic4/Wm4ConvexHull3.cpp b/src/Mod/Mesh/App/WildMagic4/Wm4ConvexHull3.cpp new file mode 100644 index 0000000000..cbd6b94061 --- /dev/null +++ b/src/Mod/Mesh/App/WildMagic4/Wm4ConvexHull3.cpp @@ -0,0 +1,591 @@ +// Geometric Tools, LLC +// Copyright (c) 1998-2010 +// Distributed under the Boost Software License, Version 1.0. +// http://www.boost.org/LICENSE_1_0.txt +// http://www.geometrictools.com/License/Boost/LICENSE_1_0.txt +// +// File Version: 4.10.0 (2009/11/18) + +#include "Wm4FoundationPCH.h" +#include "Wm4ConvexHull3.h" +#include "Wm4Mapper3.h" +#include "Wm4Query3Filtered.h" +#include "Wm4Query3Int64.h" +#include "Wm4Query3TInteger.h" +#include "Wm4Query3TRational.h" + +namespace Wm4 +{ + +//---------------------------------------------------------------------------- +template +ConvexHull3::ConvexHull3 (int iVertexQuantity, Vector3* akVertex, + Real fEpsilon, bool bOwner, Query::Type eQueryType) + : + ConvexHull(iVertexQuantity,fEpsilon,bOwner,eQueryType), + m_kLineOrigin(Vector3::ZERO), + m_kLineDirection(Vector3::ZERO), + m_kPlaneOrigin(Vector3::ZERO) +{ + assert(akVertex); + m_akVertex = akVertex; + m_akPlaneDirection[0] = Vector3::ZERO; + m_akPlaneDirection[1] = Vector3::ZERO; + m_akSVertex = 0; + m_pkQuery = 0; + + Mapper3 kMapper(m_iVertexQuantity,m_akVertex,m_fEpsilon); + if (kMapper.GetDimension() == 0) + { + // The values of m_iDimension, m_aiIndex, and m_aiAdjacent were + // already initialized by the ConvexHull base class. + return; + } + + if (kMapper.GetDimension() == 1) + { + // The set is (nearly) collinear. The caller is responsible for + // creating a ConvexHull1 object. + m_iDimension = 1; + m_kLineOrigin = kMapper.GetOrigin(); + m_kLineDirection = kMapper.GetDirection(0); + return; + } + + if (kMapper.GetDimension() == 2) + { + // The set is (nearly) coplanar. The caller is responsible for + // creating a ConvexHull2 object. + m_iDimension = 2; + m_kPlaneOrigin = kMapper.GetOrigin(); + m_akPlaneDirection[0] = kMapper.GetDirection(0); + m_akPlaneDirection[1] = kMapper.GetDirection(1); + return; + } + + m_iDimension = 3; + + int i0 = kMapper.GetExtremeIndex(0); + int i1 = kMapper.GetExtremeIndex(1); + int i2 = kMapper.GetExtremeIndex(2); + int i3 = kMapper.GetExtremeIndex(3); + + m_akSVertex = WM4_NEW Vector3[m_iVertexQuantity]; + int i; + + if (eQueryType != Query::QT_RATIONAL && eQueryType != Query::QT_FILTERED) + { + // Transform the vertices to the cube [0,1]^3. + Vector3 kMin = kMapper.GetMin(); + Real fScale = ((Real)1.0)/kMapper.GetMaxRange(); + for (i = 0; i < m_iVertexQuantity; i++) + { + m_akSVertex[i] = (m_akVertex[i] - kMin)*fScale; + } + + Real fExpand; + if (eQueryType == Query::QT_INT64) + { + // Scale the vertices to the square [0,2^{20}]^2 to allow use of + // 64-bit integers. + fExpand = (Real)(1 << 20); + m_pkQuery = WM4_NEW Query3Int64(m_iVertexQuantity, + m_akSVertex); + } + else if (eQueryType == Query::QT_INTEGER) + { + // Scale the vertices to the square [0,2^{24}]^2 to allow use of + // TInteger. + fExpand = (Real)(1 << 24); + m_pkQuery = WM4_NEW Query3TInteger(m_iVertexQuantity, + m_akSVertex); + } + else // eQueryType == Query::QT_REAL + { + // No scaling for floating point. + fExpand = (Real)1.0; + m_pkQuery = WM4_NEW Query3(m_iVertexQuantity,m_akSVertex); + } + + for (i = 0; i < m_iVertexQuantity; i++) + { + m_akSVertex[i] *= fExpand; + } + } + else + { + // No transformation needed for exact rational arithmetic or filtered + // predicates. + size_t uiSize = m_iVertexQuantity*sizeof(Vector3); + System::Memcpy(m_akSVertex,uiSize,m_akVertex,uiSize); + + if (eQueryType == Query::QT_RATIONAL) + { + m_pkQuery = WM4_NEW Query3TRational(m_iVertexQuantity, + m_akSVertex); + } + else // eQueryType == Query::QT_FILTERED + { + m_pkQuery = WM4_NEW Query3Filtered(m_iVertexQuantity, + m_akSVertex,m_fEpsilon); + } + } + + Triangle* pkT0; + Triangle* pkT1; + Triangle* pkT2; + Triangle* pkT3; + + if (kMapper.GetExtremeCCW()) + { + pkT0 = WM4_NEW Triangle(i0,i1,i3); + pkT1 = WM4_NEW Triangle(i0,i2,i1); + pkT2 = WM4_NEW Triangle(i0,i3,i2); + pkT3 = WM4_NEW Triangle(i1,i2,i3); + pkT0->AttachTo(pkT1,pkT3,pkT2); + pkT1->AttachTo(pkT2,pkT3,pkT0); + pkT2->AttachTo(pkT0,pkT3,pkT1); + pkT3->AttachTo(pkT1,pkT2,pkT0); + } + else + { + pkT0 = WM4_NEW Triangle(i0,i3,i1); + pkT1 = WM4_NEW Triangle(i0,i1,i2); + pkT2 = WM4_NEW Triangle(i0,i2,i3); + pkT3 = WM4_NEW Triangle(i1,i3,i2); + pkT0->AttachTo(pkT2,pkT3,pkT1); + pkT1->AttachTo(pkT0,pkT3,pkT2); + pkT2->AttachTo(pkT1,pkT3,pkT0); + pkT3->AttachTo(pkT0,pkT2,pkT1); + } + + m_kHull.clear(); + m_kHull.insert(pkT0); + m_kHull.insert(pkT1); + m_kHull.insert(pkT2); + m_kHull.insert(pkT3); + + for (i = 0; i < m_iVertexQuantity; i++) + { + if (!Update(i)) + { + DeleteHull(); + return; + } + } + + ExtractIndices(); +} +//---------------------------------------------------------------------------- +template +ConvexHull3::~ConvexHull3 () +{ + if (m_bOwner) + { + WM4_DELETE[] m_akVertex; + } + WM4_DELETE[] m_akSVertex; + WM4_DELETE m_pkQuery; +} +//---------------------------------------------------------------------------- +template +const Vector3& ConvexHull3::GetLineOrigin () const +{ + return m_kLineOrigin; +} +//---------------------------------------------------------------------------- +template +const Vector3& ConvexHull3::GetLineDirection () const +{ + return m_kLineDirection; +} +//---------------------------------------------------------------------------- +template +const Vector3& ConvexHull3::GetPlaneOrigin () const +{ + return m_kPlaneOrigin; +} +//---------------------------------------------------------------------------- +template +const Vector3& ConvexHull3::GetPlaneDirection (int i) const +{ + assert(0 <= i && i < 2); + return m_akPlaneDirection[i]; +} +//---------------------------------------------------------------------------- +template +ConvexHull1* ConvexHull3::GetConvexHull1 () const +{ + assert(m_iDimension == 1); + if (m_iDimension != 1) + { + return 0; + } + + Real* afProjection = WM4_NEW Real[m_iVertexQuantity]; + for (int i = 0; i < m_iVertexQuantity; i++) + { + Vector3 kDiff = m_akVertex[i] - m_kLineOrigin; + afProjection[i] = m_kLineDirection.Dot(kDiff); + } + + return WM4_NEW ConvexHull1(m_iVertexQuantity,afProjection, + m_fEpsilon,true,m_eQueryType); +} +//---------------------------------------------------------------------------- +template +ConvexHull2* ConvexHull3::GetConvexHull2 () const +{ + assert(m_iDimension == 2); + if (m_iDimension != 2) + { + return 0; + } + + Vector2* akProjection = WM4_NEW Vector2[m_iVertexQuantity]; + for (int i = 0; i < m_iVertexQuantity; i++) + { + Vector3 kDiff = m_akVertex[i] - m_kPlaneOrigin; + akProjection[i][0] = m_akPlaneDirection[0].Dot(kDiff); + akProjection[i][1] = m_akPlaneDirection[1].Dot(kDiff); + } + + return WM4_NEW ConvexHull2(m_iVertexQuantity,akProjection, + m_fEpsilon,true,m_eQueryType); +} +//---------------------------------------------------------------------------- +template +bool ConvexHull3::Update (int i) +{ + // Locate a triangle visible to the input point (if possible). + Triangle* pkVisible = 0; + Triangle* pkTri; + typename std::set::iterator pkIter; + for (pkIter = m_kHull.begin(); pkIter != m_kHull.end(); pkIter++) + { + pkTri = *pkIter; + if (pkTri->GetSign(i,m_pkQuery) > 0) + { + pkVisible = pkTri; + break; + } + } + + if (!pkVisible) + { + // The point is inside the current hull; nothing to do. + return true; + } + + // Locate and remove the visible triangles. + std::stack kVisible; + std::map kTerminator; + kVisible.push(pkVisible); + pkVisible->OnStack = true; + int j, iV0, iV1; + while (!kVisible.empty()) + { + pkTri = kVisible.top(); + kVisible.pop(); + pkTri->OnStack = false; + for (j = 0; j < 3; j++) + { + Triangle* pkAdj = pkTri->A[j]; + if (pkAdj) + { + // Detach triangle and adjacent triangle from each other. + int iNullIndex = pkTri->DetachFrom(j,pkAdj); + + if (pkAdj->GetSign(i,m_pkQuery) > 0) + { + if (!pkAdj->OnStack) + { + // Adjacent triangle is visible. + kVisible.push(pkAdj); + pkAdj->OnStack = true; + } + } + else + { + // Adjacent triangle is invisible. + iV0 = pkTri->V[j]; + iV1 = pkTri->V[(j+1)%3]; + kTerminator[iV0] = TerminatorData(iV0,iV1,iNullIndex, + pkAdj); + } + } + } + m_kHull.erase(pkTri); + WM4_DELETE pkTri; + } + + // Insert the new edges formed by the input point and the terminator + // between visible and invisible triangles. + int iSize = (int)kTerminator.size(); + assert(iSize >= 3); + typename std::map::iterator pkEdge = + kTerminator.begin(); + iV0 = pkEdge->second.V[0]; + iV1 = pkEdge->second.V[1]; + pkTri = WM4_NEW Triangle(i,iV0,iV1); + m_kHull.insert(pkTri); + + // save information for linking first/last inserted new triangles + int iSaveV0 = pkEdge->second.V[0]; + Triangle* pkSaveTri = pkTri; + + // establish adjacency links across terminator edge + pkTri->A[1] = pkEdge->second.Tri; + pkEdge->second.Tri->A[pkEdge->second.NullIndex] = pkTri; + for (j = 1; j < iSize; j++) + { + pkEdge = kTerminator.find(iV1); + assert(pkEdge != kTerminator.end()); + iV0 = iV1; + iV1 = pkEdge->second.V[1]; + Triangle* pkNext = WM4_NEW Triangle(i,iV0,iV1); + m_kHull.insert(pkNext); + + // establish adjacency links across terminator edge + pkNext->A[1] = pkEdge->second.Tri; + pkEdge->second.Tri->A[pkEdge->second.NullIndex] = pkNext; + + // establish adjacency links with previously inserted triangle + pkNext->A[0] = pkTri; + pkTri->A[2] = pkNext; + + pkTri = pkNext; + } + assert(iV1 == iSaveV0); + (void)iSaveV0; // avoid warning in Release build + + // establish adjacency links between first/last triangles + pkSaveTri->A[0] = pkTri; + pkTri->A[2] = pkSaveTri; + + return true; +} +//---------------------------------------------------------------------------- +template +void ConvexHull3::ExtractIndices () +{ + int iTQuantity = (int)m_kHull.size(); + m_iSimplexQuantity = iTQuantity; + m_aiIndex = WM4_NEW int[3*m_iSimplexQuantity]; + + typename std::set::iterator pkIter; + int i = 0; + for (pkIter = m_kHull.begin(); pkIter != m_kHull.end(); pkIter++) + { + Triangle* pkTri = *pkIter; + for (int j = 0; j < 3; j++) + { + m_aiIndex[i++] = pkTri->V[j]; + } + WM4_DELETE pkTri; + } + m_kHull.clear(); +} +//---------------------------------------------------------------------------- +template +void ConvexHull3::DeleteHull () +{ + typename std::set::iterator pkIter; + for (pkIter = m_kHull.begin(); pkIter != m_kHull.end(); pkIter++) + { + Triangle* pkTri = *pkIter; + WM4_DELETE pkTri; + } + m_kHull.clear(); +} +//---------------------------------------------------------------------------- +template +ConvexHull3::ConvexHull3 (const char* acFilename) + : + ConvexHull(0,(Real)0.0,false,Query::QT_REAL) +{ + m_akVertex = 0; + m_akSVertex = 0; + m_pkQuery = 0; + bool bLoaded = Load(acFilename); + assert(bLoaded); + (void)bLoaded; // avoid warning in Release build +} +//---------------------------------------------------------------------------- +template +bool ConvexHull3::Load (const char* acFilename) +{ + FILE* pkIFile = System::Fopen(acFilename,"rb"); + if (!pkIFile) + { + return false; + } + + ConvexHull::Load(pkIFile); + + WM4_DELETE m_pkQuery; + WM4_DELETE[] m_akSVertex; + if (m_bOwner) + { + WM4_DELETE[] m_akVertex; + } + + m_bOwner = true; + m_akVertex = WM4_NEW Vector3[m_iVertexQuantity]; + m_akSVertex = WM4_NEW Vector3[m_iVertexQuantity+4]; + + size_t uiSize = sizeof(Real); + int iVQ = 3*m_iVertexQuantity; + if (uiSize == 4) + { + System::Read4le(pkIFile,iVQ,m_akVertex); + System::Read4le(pkIFile,iVQ,m_akSVertex); + System::Read4le(pkIFile,3,(Real*)m_kLineOrigin); + System::Read4le(pkIFile,3,(Real*)m_kLineDirection); + System::Read4le(pkIFile,3,(Real*)m_kPlaneOrigin); + System::Read4le(pkIFile,3,(Real*)m_akPlaneDirection[0]); + System::Read4le(pkIFile,3,(Real*)m_akPlaneDirection[1]); + } + else // iSize == 8 + { + System::Read8le(pkIFile,iVQ,m_akVertex); + System::Read8le(pkIFile,iVQ,m_akSVertex); + System::Read8le(pkIFile,3,(Real*)m_kLineOrigin); + System::Read8le(pkIFile,3,(Real*)m_kLineDirection); + System::Read8le(pkIFile,3,(Real*)m_kPlaneOrigin); + System::Read8le(pkIFile,3,(Real*)m_akPlaneDirection[0]); + System::Read8le(pkIFile,3,(Real*)m_akPlaneDirection[1]); + } + + System::Fclose(pkIFile); + + switch (m_eQueryType) + { + case Query::QT_INT64: + m_pkQuery = WM4_NEW Query3Int64(m_iVertexQuantity,m_akSVertex); + break; + case Query::QT_INTEGER: + m_pkQuery = WM4_NEW Query3TInteger(m_iVertexQuantity, + m_akSVertex); + break; + case Query::QT_RATIONAL: + m_pkQuery = WM4_NEW Query3TRational(m_iVertexQuantity, + m_akSVertex); + break; + case Query::QT_REAL: + m_pkQuery = WM4_NEW Query3(m_iVertexQuantity,m_akSVertex); + break; + case Query::QT_FILTERED: + m_pkQuery = WM4_NEW Query3Filtered(m_iVertexQuantity, + m_akSVertex,m_fEpsilon); + break; + } + + return true; +} +//---------------------------------------------------------------------------- +template +bool ConvexHull3::Save (const char* acFilename) const +{ + FILE* pkOFile = System::Fopen(acFilename,"wb"); + if (!pkOFile) + { + return false; + } + + ConvexHull::Save(pkOFile); + + size_t uiSize = sizeof(Real); + int iVQ = 3*m_iVertexQuantity; + if (uiSize == 4) + { + System::Write4le(pkOFile,iVQ,m_akVertex); + System::Write4le(pkOFile,iVQ,m_akSVertex); + System::Write4le(pkOFile,3,(const Real*)m_kLineOrigin); + System::Write4le(pkOFile,3,(const Real*)m_kLineDirection); + System::Write4le(pkOFile,3,(const Real*)m_kPlaneOrigin); + System::Write4le(pkOFile,3,(const Real*)m_akPlaneDirection[0]); + System::Write4le(pkOFile,3,(const Real*)m_akPlaneDirection[1]); + } + else // iSize == 8 + { + System::Write8le(pkOFile,iVQ,m_akVertex); + System::Write8le(pkOFile,iVQ,m_akSVertex); + System::Write8le(pkOFile,3,(const Real*)m_kLineOrigin); + System::Write8le(pkOFile,3,(const Real*)m_kLineDirection); + System::Write8le(pkOFile,3,(const Real*)m_kPlaneOrigin); + System::Write8le(pkOFile,3,(const Real*)m_akPlaneDirection[0]); + System::Write8le(pkOFile,3,(const Real*)m_akPlaneDirection[1]); + } + + System::Fclose(pkOFile); + return true; +} +//---------------------------------------------------------------------------- + +//---------------------------------------------------------------------------- +// ConvexHull3::Triangle +//---------------------------------------------------------------------------- +template +ConvexHull3::Triangle::Triangle (int iV0, int iV1, int iV2) +{ + V[0] = iV0; + V[1] = iV1; + V[2] = iV2; + A[0] = 0; + A[1] = 0; + A[2] = 0; + Sign = 0; + Time = -1; + OnStack = false; +} +//---------------------------------------------------------------------------- +template +int ConvexHull3::Triangle::GetSign (int i, const Query3* pkQuery) +{ + if (i != Time) + { + Time = i; + Sign = pkQuery->ToPlane(i,V[0],V[1],V[2]); + } + + return Sign; +} +//---------------------------------------------------------------------------- +template +void ConvexHull3::Triangle::AttachTo (Triangle* pkAdj0, + Triangle* pkAdj1, Triangle* pkAdj2) +{ + // assert: The input adjacent triangles are correctly ordered. + A[0] = pkAdj0; + A[1] = pkAdj1; + A[2] = pkAdj2; +} +//---------------------------------------------------------------------------- +template +int ConvexHull3::Triangle::DetachFrom (int iAdj, Triangle* pkAdj) +{ + assert(0 <= iAdj && iAdj < 3 && A[iAdj] == pkAdj); + A[iAdj] = 0; + for (int i = 0; i < 3; i++) + { + if (pkAdj->A[i] == this) + { + pkAdj->A[i] = 0; + return i; + } + } + return -1; +} +//---------------------------------------------------------------------------- + +//---------------------------------------------------------------------------- +// explicit instantiation +//---------------------------------------------------------------------------- +template WM4_FOUNDATION_ITEM +class ConvexHull3; + +template WM4_FOUNDATION_ITEM +class ConvexHull3; +//---------------------------------------------------------------------------- +} diff --git a/src/Mod/Mesh/App/WildMagic4/Wm4ConvexHull3.h b/src/Mod/Mesh/App/WildMagic4/Wm4ConvexHull3.h new file mode 100644 index 0000000000..a5e37cd9ff --- /dev/null +++ b/src/Mod/Mesh/App/WildMagic4/Wm4ConvexHull3.h @@ -0,0 +1,126 @@ +// Geometric Tools, LLC +// Copyright (c) 1998-2010 +// Distributed under the Boost Software License, Version 1.0. +// http://www.boost.org/LICENSE_1_0.txt +// http://www.geometrictools.com/License/Boost/LICENSE_1_0.txt +// +// File Version: 4.10.0 (2009/11/18) + +#ifndef WM4CONVEXHULL3_H +#define WM4CONVEXHULL3_H + +#include "Wm4FoundationLIB.h" +#include "Wm4ConvexHull1.h" +#include "Wm4ConvexHull2.h" +#include "Wm4Query3.h" + +namespace Wm4 +{ + +template +class WM4_FOUNDATION_ITEM ConvexHull3 : public ConvexHull +{ +public: + // The input to the constructor is the array of vertices whose convex hull + // is required. If you want ConvexHull3 to delete the vertices during + // destruction, set bOwner to 'true'. Otherwise, you own the vertices and + // must delete them yourself. + // + // You have a choice of speed versus accuracy. The fastest choice is + // Query::QT_INT64, but it gives up a lot of precision, scaling the points + // to [0,2^{20}]^3. The choice Query::QT_INTEGER gives up less precision, + // scaling the points to [0,2^{24}]^3. The choice Query::QT_RATIONAL uses + // exact arithmetic, but is the slowest choice. The choice Query::QT_REAL + // uses floating-point arithmetic, but is not robust in all cases. + + ConvexHull3 (int iVertexQuantity, Vector3* akVertex, Real fEpsilon, + bool bOwner, Query::Type eQueryType); + virtual ~ConvexHull3 (); + + // If GetDimension() returns 1, then the points lie on a line. You must + // create a ConvexHull1 object using the function provided. + const Vector3& GetLineOrigin () const; + const Vector3& GetLineDirection () const; + ConvexHull1* GetConvexHull1 () const; + + // If GetDimension() returns 2, then the points lie on a plane. The plane + // has two direction vectors (inputs 0 or 1). You must create a + // ConvexHull2 object using the function provided. + const Vector3& GetPlaneOrigin () const; + const Vector3& GetPlaneDirection (int i) const; + ConvexHull2* GetConvexHull2 () const; + + // Support for streaming to/from disk. + ConvexHull3 (const char* acFilename); + bool Load (const char* acFilename); + bool Save (const char* acFilename) const; + +private: + using ConvexHull::m_eQueryType; + using ConvexHull::m_iVertexQuantity; + using ConvexHull::m_iDimension; + using ConvexHull::m_iSimplexQuantity; + using ConvexHull::m_aiIndex; + using ConvexHull::m_fEpsilon; + using ConvexHull::m_bOwner; + + class WM4_FOUNDATION_ITEM Triangle + { + public: + Triangle (int iV0, int iV1, int iV2); + + int GetSign (int i, const Query3* pkQuery); + void AttachTo (Triangle* pkAdj0, Triangle* pkAdj1, Triangle* pkAdj2); + int DetachFrom (int iAdj, Triangle* pkAdj); + + int V[3]; + Triangle* A[3]; + int Sign; + int Time; + bool OnStack; + }; + + bool Update (int i); + void ExtractIndices (); + void DeleteHull (); + + // The input points. + Vector3* m_akVertex; + + // Support for robust queries. + Vector3* m_akSVertex; + Query3* m_pkQuery; + + // The line of containment if the dimension is 1. + Vector3 m_kLineOrigin, m_kLineDirection; + + // The plane of containment if the dimension is 2. + Vector3 m_kPlaneOrigin, m_akPlaneDirection[2]; + + // The current hull. + std::set m_kHull; + + class WM4_FOUNDATION_ITEM TerminatorData + { + public: + TerminatorData (int iV0 = -1, int iV1 = -1, int iNullIndex = -1, + Triangle* pkTri = 0) + { + V[0] = iV0; + V[1] = iV1; + NullIndex = iNullIndex; + Tri = pkTri; + } + + int V[2]; + int NullIndex; + Triangle* Tri; + }; +}; + +typedef ConvexHull3 ConvexHull3f; +typedef ConvexHull3 ConvexHull3d; + +} + +#endif diff --git a/src/Mod/Mesh/App/WildMagic4/Wm4Quaternion.cpp b/src/Mod/Mesh/App/WildMagic4/Wm4Quaternion.cpp new file mode 100644 index 0000000000..6acbcb0b7c --- /dev/null +++ b/src/Mod/Mesh/App/WildMagic4/Wm4Quaternion.cpp @@ -0,0 +1,31 @@ +// Geometric Tools, LLC +// Copyright (c) 1998-2010 +// Distributed under the Boost Software License, Version 1.0. +// http://www.boost.org/LICENSE_1_0.txt +// http://www.geometrictools.com/License/Boost/LICENSE_1_0.txt +// +// File Version: 4.10.0 (2009/11/18) + +#include "Wm4FoundationPCH.h" +#include "Wm4Quaternion.h" + +namespace Wm4 +{ +template<> const Quaternion + Quaternion::IDENTITY(1.0f,0.0f,0.0f,0.0f); +template<> const Quaternion + Quaternion::ZERO(0.0f,0.0f,0.0f,0.0f); +template<> int Quaternion::ms_iNext[3] = { 1, 2, 0 }; +template<> float Quaternion::ms_fTolerance = 1e-06f; +template<> float Quaternion::ms_fRootTwo = (float)sqrt(2.0); +template<> float Quaternion::ms_fRootHalf = (float)sqrt(0.5); + +template<> const Quaternion + Quaternion::IDENTITY(1.0,0.0,0.0,0.0); +template<> const Quaternion + Quaternion::ZERO(0.0,0.0,0.0,0.0); +template<> int Quaternion::ms_iNext[3] = { 1, 2, 0 }; +template<> double Quaternion::ms_fTolerance = 1e-08; +template<> double Quaternion::ms_fRootTwo = sqrt(2.0); +template<> double Quaternion::ms_fRootHalf = sqrt(0.5); +} diff --git a/src/Mod/Mesh/App/WildMagic4/Wm4Quaternion.h b/src/Mod/Mesh/App/WildMagic4/Wm4Quaternion.h new file mode 100644 index 0000000000..70114bca60 --- /dev/null +++ b/src/Mod/Mesh/App/WildMagic4/Wm4Quaternion.h @@ -0,0 +1,326 @@ +// Geometric Tools, LLC +// Copyright (c) 1998-2010 +// Distributed under the Boost Software License, Version 1.0. +// http://www.boost.org/LICENSE_1_0.txt +// http://www.geometrictools.com/License/Boost/LICENSE_1_0.txt +// +// File Version: 4.10.0 (2009/11/18) + +#ifndef WM4QUATERNION_H +#define WM4QUATERNION_H + +#include "Wm4FoundationLIB.h" +#include "Wm4Matrix3.h" + +namespace Wm4 +{ + +template +class Quaternion +{ +public: + // A quaternion is q = w + x*i + y*j + z*k where (w,x,y,z) is not + // necessarily a unit length vector in 4D. + + // construction + Quaternion (); // uninitialized + Quaternion (Real fW, Real fX, Real fY, Real fZ); + Quaternion (const Quaternion& rkQ); + + // quaternion for the input rotation matrix + Quaternion (const Matrix3& rkRot); + + // quaternion for the rotation of the axis-angle pair + Quaternion (const Vector3& rkAxis, Real fAngle); + + // quaternion for the rotation matrix with specified columns + Quaternion (const Vector3 akRotColumn[3]); + + // member access: 0 = w, 1 = x, 2 = y, 3 = z + inline operator const Real* () const; + inline operator Real* (); + inline Real operator[] (int i) const; + inline Real& operator[] (int i); + inline Real W () const; + inline Real& W (); + inline Real X () const; + inline Real& X (); + inline Real Y () const; + inline Real& Y (); + inline Real Z () const; + inline Real& Z (); + + // assignment + inline Quaternion& operator= (const Quaternion& rkQ); + + // comparison + bool operator== (const Quaternion& rkQ) const; + bool operator!= (const Quaternion& rkQ) const; + bool operator< (const Quaternion& rkQ) const; + bool operator<= (const Quaternion& rkQ) const; + bool operator> (const Quaternion& rkQ) const; + bool operator>= (const Quaternion& rkQ) const; + + // arithmetic operations + inline Quaternion operator+ (const Quaternion& rkQ) const; + inline Quaternion operator- (const Quaternion& rkQ) const; + inline Quaternion operator* (const Quaternion& rkQ) const; + inline Quaternion operator* (Real fScalar) const; + inline Quaternion operator/ (Real fScalar) const; + inline Quaternion operator- () const; + + // arithmetic updates + inline Quaternion& operator+= (const Quaternion& rkQ); + inline Quaternion& operator-= (const Quaternion& rkQ); + inline Quaternion& operator*= (Real fScalar); + inline Quaternion& operator/= (Real fScalar); + + // conversion between quaternions, matrices, and axis-angle + Quaternion& FromRotationMatrix (const Matrix3& rkRot); + void ToRotationMatrix (Matrix3& rkRot) const; + Quaternion& FromRotationMatrix (const Vector3 akRotColumn[3]); + void ToRotationMatrix (Vector3 akRotColumn[3]) const; + Quaternion& FromAxisAngle (const Vector3& rkAxis, Real fAngle); + void ToAxisAngle (Vector3& rkAxis, Real& rfAngle) const; + + // functions of a quaternion + inline Real Length () const; // length of 4-tuple + inline Real SquaredLength () const; // squared length of 4-tuple + inline Real Dot (const Quaternion& rkQ) const; // dot product of 4-tuples + inline Real Normalize (); // make the 4-tuple unit length + Quaternion Inverse () const; // apply to non-zero quaternion + Quaternion Conjugate () const; + Quaternion Exp () const; // apply to quaternion with w = 0 + Quaternion Log () const; // apply to unit-length quaternion + + // rotation of a vector by a quaternion + Vector3 Rotate (const Vector3& rkVector) const; + + // spherical linear interpolation + Quaternion& Slerp (Real fT, const Quaternion& rkP, const Quaternion& rkQ); + + Quaternion& SlerpExtraSpins (Real fT, const Quaternion& rkP, + const Quaternion& rkQ, int iExtraSpins); + + // intermediate terms for spherical quadratic interpolation + Quaternion& Intermediate (const Quaternion& rkQ0, + const Quaternion& rkQ1, const Quaternion& rkQ2); + + // spherical quadratic interpolation + Quaternion& Squad (Real fT, const Quaternion& rkQ0, + const Quaternion& rkA0, const Quaternion& rkA1, + const Quaternion& rkQ1); + + // Compute a quaternion that rotates unit-length vector V1 to unit-length + // vector V2. The rotation is about the axis perpendicular to both V1 and + // V2, with angle of that between V1 and V2. If V1 and V2 are parallel, + // any axis of rotation will do, such as the permutation (z2,x2,y2), where + // V2 = (x2,y2,z2). + Quaternion& Align (const Vector3& rkV1, const Vector3& rkV2); + + // Decompose a quaternion into q = q_twist * q_swing, where q is 'this' + // quaternion. If V1 is the input axis and V2 is the rotation of V1 by + // q, q_swing represents the rotation about the axis perpendicular to + // V1 and V2 (see Quaternion::Align), and q_twist is a rotation about V1. + void DecomposeTwistTimesSwing (const Vector3& rkV1, + Quaternion& rkTwist, Quaternion& rkSwing); + + // Decompose a quaternion into q = q_swing * q_twist, where q is 'this' + // quaternion. If V1 is the input axis and V2 is the rotation of V1 by + // q, q_swing represents the rotation about the axis perpendicular to + // V1 and V2 (see Quaternion::Align), and q_twist is a rotation about V1. + void DecomposeSwingTimesTwist (const Vector3& rkV1, + Quaternion& rkSwing, Quaternion& rkTwist); + + // *** Find closest quaternions with unconstrained angles. + + // Closest quaternion of the form (cx + sx*i). + Quaternion GetClosestX () const; + + // Closest quaternion of the form (cy + sy*j). + Quaternion GetClosestY () const; + + // Closest quaternion of the form (cz + sz*k). + Quaternion GetClosestZ () const; + + // Closest quaternion of the form (cx + sx*i)*(cy + sy*j). + Quaternion GetClosestXY () const; + + // Closest quaternion of the form (cy + sy*j)*(cx + sx*i). + Quaternion GetClosestYX () const; + + // Closest quaternion of the form (cz + sz*k)*(cx + sx*i). + Quaternion GetClosestZX () const; + + // Closest quaternion of the form (cx + sx*i)*(cz + sz*k). + Quaternion GetClosestXZ () const; + + // Closest quaternion of the form (cy + sy*j)*(cz + sz*k). + Quaternion GetClosestYZ () const; + + // Closest quaternion of the form (cz + sz*k)*(cy + sy*j). + Quaternion GetClosestZY () const; + + // Factor to (cx + sx*i)*(cy + sy*j)*(cz + sz*k). + void FactorXYZ (Real& rfCx, Real& rfSx, Real& rfCy, Real& rfSy, + Real& rfCz, Real& rfSz); + + // Factor to (cx + sx*i)*(cz + sz*k)*(cy + sy*j). + void FactorXZY (Real& rfCx, Real& rfSx, Real& rfCz, Real& rfSz, + Real& rfCy, Real& rfSy); + + // Factor to (cy + sy*j)*(cz + sz*k)*(cx + sx*i). + void FactorYZX (Real& rfCy, Real& rfSy, Real& rfCz, Real& rfSz, + Real& rfCx, Real& rfSx); + + // Factor to (cy + sy*j)*(cx + sx*i)*(cz + sz*k). + void FactorYXZ (Real& rfCy, Real& rfSy, Real& rfCx, Real& rfSx, + Real& rfCz, Real& rfSz); + + // Factor to (cz + sz*k)*(cx + sx*i)*(cy + sy*j). + void FactorZXY (Real& rfCz, Real& rfSz, Real& rfCx, Real& rfSx, + Real& rfCy, Real& rfSy); + + // Factor to (cz + sz*k)*(cy + sy*j)*(cx + sx*i). + void FactorZYX (Real& rfCz, Real& rfSz, Real& rfCy, Real& rfSy, + Real& rfCx, Real& rfSx); + + // *** Find closest quaternions with constrained angles. + class Constraints + { + public: + Constraints () + { + // Members are uninitialized. + } + + Constraints (Real fMinAngle, Real fMaxAngle) + { + SetAngles(fMinAngle,fMaxAngle); + } + + void SetAngles (Real fMinAngle, Real fMaxAngle) + { + m_fMinAngle = fMinAngle; + m_fMaxAngle = fMaxAngle; + m_fCosMinAngle = Math::Cos(m_fMinAngle); + m_fSinMinAngle = Math::Sin(m_fMinAngle); + m_fCosMaxAngle = Math::Cos(m_fMaxAngle); + m_fSinMaxAngle = Math::Sin(m_fMaxAngle); + m_fDiffCosMaxMin = m_fCosMaxAngle - m_fCosMinAngle; + m_fDiffSinMaxMin = m_fSinMaxAngle - m_fSinMinAngle; + Real fAvrAngle = ((Real)0.5)*(m_fMinAngle + m_fMaxAngle); + m_fCosAvrAngle = Math::Cos(fAvrAngle); + m_fSinAvrAngle = Math::Sin(fAvrAngle); + } + + bool IsValid (Real fX, Real fY) const + { + // (x,y) must be unit-length. + + // Test whether (x,y) satisfies the constraints. + Real fXm = fX - m_fCosMinAngle; + Real fYm = fY - m_fSinMinAngle; + if (fXm*m_fDiffSinMaxMin >= fYm*m_fDiffCosMaxMin) + { + return true; + } + + // Test whether (-x,-y) satisfies the constraints. + Real fXp = fX + m_fCosMinAngle; + Real fYp = fY + m_fSinMinAngle; + if (fXp*m_fDiffSinMaxMin <= fYp*m_fDiffCosMaxMin) + { + return true; + } + + return false; + } + + Real m_fMinAngle; // in [-PI/2,PI/2] + Real m_fMaxAngle; // in [m_fMinAngle/2,PI/2] + Real m_fCosMinAngle; // = cos(m_fMinAngle) + Real m_fSinMinAngle; // = sin(m_fMinAngle) + Real m_fCosMaxAngle; // = cos(m_fMaxAngle) + Real m_fSinMaxAngle; // = sin(m_fMaxAngle) + Real m_fDiffCosMaxMin; // = cos(m_fMaxAngle) - cos(m_fMinAngle) + Real m_fDiffSinMaxMin; // = sin(m_fMaxAngle) - sin(m_fMinAngle) + Real m_fCosAvrAngle; // = cos((m_fMinAngle + m_fMaxAngle)/2) + Real m_fSinAvrAngle; // = sin((m_fMinAngle + mM_faxAngle)/2) + }; + + // Closest constrained quaternion of the form (cx + sx*i). + Quaternion GetClosestX (const Constraints& rkXCon) const; + + // Closest constrained quaternion of the form (cy + sy*j). + Quaternion GetClosestY (const Constraints& rkYCon) const; + + // Closest constrained quaternion of the form (cz + sz*k). + Quaternion GetClosestZ (const Constraints& rkZCon) const; + + // Closest constrained quaternion of the form (cx + sx*i)*(cy + sy*j). + Quaternion GetClosestXY (const Constraints& rkXCon, + const Constraints& rkYCon) const; + + // Closest constrained quaternion of the form (cy + sy*j)*(cx + sx*i). + Quaternion GetClosestYX (const Constraints& rkYCon, + const Constraints& rkXCon) const; + + // Closest constrained quaternion of the form (cz + sz*k)*(cx + sx*i). + Quaternion GetClosestZX (const Constraints& rkZCon, + const Constraints& rkXCon) const; + + // Closest constrained quaternion of the form (cx + sx*i)*(cz + sz*k). + Quaternion GetClosestXZ (const Constraints& rkXCon, + const Constraints& rkZCon) const; + + // Closest constrained quaternion of the form (cz + sz*k)*(cy + sy*j). + Quaternion GetClosestZY (const Constraints& rkZCon, + const Constraints& rkYCon) const; + + // Closest constrained quaternion of the form (cy + sy*j)*(cz + sz*k). + Quaternion GetClosestYZ (const Constraints& rkYCon, + const Constraints& rkZCon) const; + + // special values + WM4_FOUNDATION_ITEM static const Quaternion IDENTITY; + WM4_FOUNDATION_ITEM static const Quaternion ZERO; + +private: + // support for comparisons + int CompareArrays (const Quaternion& rkQ) const; + + // Closest unconstrained quaternion of the form: + // (cx + sx*i) when iAxis = 1, + // (cy + sy*j) when iAxis = 2, + // (cz + sz*k) when iAxis = 3 + Quaternion GetClosest (int iAxis) const; + + // Closest constrained quaternion of the form: + // (cx + sx*i) when iAxis = 1, + // (cy + sy*j) when iAxis = 2, + // (cz + sz*k) when iAxis = 3 + Quaternion GetClosest (int iAxis, const Constraints& rkCon) const; + + // support for FromRotationMatrix + WM4_FOUNDATION_ITEM static int ms_iNext[3]; + + // support for closest quaternions + WM4_FOUNDATION_ITEM static Real ms_fTolerance; + WM4_FOUNDATION_ITEM static Real ms_fRootTwo; + WM4_FOUNDATION_ITEM static Real ms_fRootHalf; + + Real m_afTuple[4]; +}; + +template +inline Quaternion operator* (Real fScalar, const Quaternion& rkQ); + +#include "Wm4Quaternion.inl" + +typedef Quaternion Quaternionf; +typedef Quaternion Quaterniond; + +} + +#endif diff --git a/src/Mod/Mesh/App/WildMagic4/Wm4Quaternion.inl b/src/Mod/Mesh/App/WildMagic4/Wm4Quaternion.inl new file mode 100644 index 0000000000..894998d1d7 --- /dev/null +++ b/src/Mod/Mesh/App/WildMagic4/Wm4Quaternion.inl @@ -0,0 +1,1799 @@ +// Geometric Tools, LLC +// Copyright (c) 1998-2010 +// Distributed under the Boost Software License, Version 1.0. +// http://www.boost.org/LICENSE_1_0.txt +// http://www.geometrictools.com/License/Boost/LICENSE_1_0.txt +// +// File Version: 4.10.0 (2009/11/18) + +//---------------------------------------------------------------------------- +template +Quaternion::Quaternion () +{ + // uninitialized for performance in array construction +} +//---------------------------------------------------------------------------- +template +Quaternion::Quaternion (Real fW, Real fX, Real fY, Real fZ) +{ + m_afTuple[0] = fW; + m_afTuple[1] = fX; + m_afTuple[2] = fY; + m_afTuple[3] = fZ; +} +//---------------------------------------------------------------------------- +template +Quaternion::Quaternion (const Quaternion& rkQ) +{ + m_afTuple[0] = rkQ.m_afTuple[0]; + m_afTuple[1] = rkQ.m_afTuple[1]; + m_afTuple[2] = rkQ.m_afTuple[2]; + m_afTuple[3] = rkQ.m_afTuple[3]; +} +//---------------------------------------------------------------------------- +template +Quaternion::Quaternion (const Matrix3& rkRot) +{ + FromRotationMatrix(rkRot); +} +//---------------------------------------------------------------------------- +template +Quaternion::Quaternion (const Vector3& rkAxis, Real fAngle) +{ + FromAxisAngle(rkAxis,fAngle); +} +//---------------------------------------------------------------------------- +template +Quaternion::Quaternion (const Vector3 akRotColumn[3]) +{ + FromRotationMatrix(akRotColumn); +} +//---------------------------------------------------------------------------- +template +inline Quaternion::operator const Real* () const +{ + return m_afTuple; +} +//---------------------------------------------------------------------------- +template +inline Quaternion::operator Real* () +{ + return m_afTuple; +} +//---------------------------------------------------------------------------- +template +inline Real Quaternion::operator[] (int i) const +{ + return m_afTuple[i]; +} +//---------------------------------------------------------------------------- +template +inline Real& Quaternion::operator[] (int i) +{ + return m_afTuple[i]; +} +//---------------------------------------------------------------------------- +template +inline Real Quaternion::W () const +{ + return m_afTuple[0]; +} +//---------------------------------------------------------------------------- +template +inline Real& Quaternion::W () +{ + return m_afTuple[0]; +} +//---------------------------------------------------------------------------- +template +inline Real Quaternion::X () const +{ + return m_afTuple[1]; +} +//---------------------------------------------------------------------------- +template +inline Real& Quaternion::X () +{ + return m_afTuple[1]; +} +//---------------------------------------------------------------------------- +template +inline Real Quaternion::Y () const +{ + return m_afTuple[2]; +} +//---------------------------------------------------------------------------- +template +inline Real& Quaternion::Y () +{ + return m_afTuple[2]; +} +//---------------------------------------------------------------------------- +template +inline Real Quaternion::Z () const +{ + return m_afTuple[3]; +} +//---------------------------------------------------------------------------- +template +inline Real& Quaternion::Z () +{ + return m_afTuple[3]; +} +//---------------------------------------------------------------------------- +template +inline Quaternion& Quaternion::operator= (const Quaternion& rkQ) +{ + m_afTuple[0] = rkQ.m_afTuple[0]; + m_afTuple[1] = rkQ.m_afTuple[1]; + m_afTuple[2] = rkQ.m_afTuple[2]; + m_afTuple[3] = rkQ.m_afTuple[3]; + return *this; +} +//---------------------------------------------------------------------------- +template +int Quaternion::CompareArrays (const Quaternion& rkQ) const +{ + return memcmp(m_afTuple,rkQ.m_afTuple,4*sizeof(Real)); +} +//---------------------------------------------------------------------------- +template +bool Quaternion::operator== (const Quaternion& rkQ) const +{ + return CompareArrays(rkQ) == 0; +} +//---------------------------------------------------------------------------- +template +bool Quaternion::operator!= (const Quaternion& rkQ) const +{ + return CompareArrays(rkQ) != 0; +} +//---------------------------------------------------------------------------- +template +bool Quaternion::operator< (const Quaternion& rkQ) const +{ + return CompareArrays(rkQ) < 0; +} +//---------------------------------------------------------------------------- +template +bool Quaternion::operator<= (const Quaternion& rkQ) const +{ + return CompareArrays(rkQ) <= 0; +} +//---------------------------------------------------------------------------- +template +bool Quaternion::operator> (const Quaternion& rkQ) const +{ + return CompareArrays(rkQ) > 0; +} +//---------------------------------------------------------------------------- +template +bool Quaternion::operator>= (const Quaternion& rkQ) const +{ + return CompareArrays(rkQ) >= 0; +} +//---------------------------------------------------------------------------- +template +inline Quaternion Quaternion::operator+ ( + const Quaternion& rkQ) const +{ + Quaternion kSum; + for (int i = 0; i < 4; i++) + { + kSum.m_afTuple[i] = m_afTuple[i] + rkQ.m_afTuple[i]; + } + return kSum; +} +//---------------------------------------------------------------------------- +template +inline Quaternion Quaternion::operator- ( + const Quaternion& rkQ) const +{ + Quaternion kDiff; + for (int i = 0; i < 4; i++) + { + kDiff.m_afTuple[i] = m_afTuple[i] - rkQ.m_afTuple[i]; + } + return kDiff; +} +//---------------------------------------------------------------------------- +template +inline Quaternion Quaternion::operator* ( + const Quaternion& rkQ) const +{ + // NOTE: Multiplication is not generally commutative, so in most + // cases p*q != q*p. + + Quaternion kProd; + + kProd.m_afTuple[0] = + m_afTuple[0]*rkQ.m_afTuple[0] - + m_afTuple[1]*rkQ.m_afTuple[1] - + m_afTuple[2]*rkQ.m_afTuple[2] - + m_afTuple[3]*rkQ.m_afTuple[3]; + + kProd.m_afTuple[1] = + m_afTuple[0]*rkQ.m_afTuple[1] + + m_afTuple[1]*rkQ.m_afTuple[0] + + m_afTuple[2]*rkQ.m_afTuple[3] - + m_afTuple[3]*rkQ.m_afTuple[2]; + + kProd.m_afTuple[2] = + m_afTuple[0]*rkQ.m_afTuple[2] + + m_afTuple[2]*rkQ.m_afTuple[0] + + m_afTuple[3]*rkQ.m_afTuple[1] - + m_afTuple[1]*rkQ.m_afTuple[3]; + + kProd.m_afTuple[3] = + m_afTuple[0]*rkQ.m_afTuple[3] + + m_afTuple[3]*rkQ.m_afTuple[0] + + m_afTuple[1]*rkQ.m_afTuple[2] - + m_afTuple[2]*rkQ.m_afTuple[1]; + + return kProd; +} +//---------------------------------------------------------------------------- +template +inline Quaternion Quaternion::operator* (Real fScalar) const +{ + Quaternion kProd; + for (int i = 0; i < 4; i++) + { + kProd.m_afTuple[i] = fScalar*m_afTuple[i]; + } + return kProd; +} +//---------------------------------------------------------------------------- +template +inline Quaternion Quaternion::operator/ (Real fScalar) const +{ + Quaternion kQuot; + int i; + + if (fScalar != (Real)0.0) + { + Real fInvScalar = ((Real)1.0)/fScalar; + for (i = 0; i < 4; i++) + { + kQuot.m_afTuple[i] = fInvScalar*m_afTuple[i]; + } + } + else + { + for (i = 0; i < 4; i++) + { + kQuot.m_afTuple[i] = Math::MAX_REAL; + } + } + + return kQuot; +} +//---------------------------------------------------------------------------- +template +inline Quaternion Quaternion::operator- () const +{ + Quaternion kNeg; + for (int i = 0; i < 4; i++) + { + kNeg.m_afTuple[i] = -m_afTuple[i]; + } + return kNeg; +} +//---------------------------------------------------------------------------- +template +inline Quaternion operator* (Real fScalar, const Quaternion& rkQ) +{ + Quaternion kProd; + for (int i = 0; i < 4; i++) + { + kProd[i] = fScalar*rkQ[i]; + } + return kProd; +} +//---------------------------------------------------------------------------- +template +inline Quaternion& Quaternion::operator+= (const Quaternion& rkQ) +{ + for (int i = 0; i < 4; i++) + { + m_afTuple[i] += rkQ.m_afTuple[i]; + } + return *this; +} +//---------------------------------------------------------------------------- +template +inline Quaternion& Quaternion::operator-= (const Quaternion& rkQ) +{ + for (int i = 0; i < 4; i++) + { + m_afTuple[i] -= rkQ.m_afTuple[i]; + } + return *this; +} +//---------------------------------------------------------------------------- +template +inline Quaternion& Quaternion::operator*= (Real fScalar) +{ + for (int i = 0; i < 4; i++) + { + m_afTuple[i] *= fScalar; + } + return *this; +} +//---------------------------------------------------------------------------- +template +inline Quaternion& Quaternion::operator/= (Real fScalar) +{ + int i; + + if (fScalar != (Real)0.0) + { + Real fInvScalar = ((Real)1.0)/fScalar; + for (i = 0; i < 4; i++) + { + m_afTuple[i] *= fInvScalar; + } + } + else + { + for (i = 0; i < 4; i++) + { + m_afTuple[i] = Math::MAX_REAL; + } + } + + return *this; +} +//---------------------------------------------------------------------------- +template +Quaternion& Quaternion::FromRotationMatrix ( + const Matrix3& rkRot) +{ + // Algorithm in Ken Shoemake's article in 1987 SIGGRAPH course notes + // article "Quaternion Calculus and Fast Animation". + + Real fTrace = rkRot(0,0) + rkRot(1,1) + rkRot(2,2); + Real fRoot; + + if (fTrace > (Real)0.0) + { + // |w| > 1/2, may as well choose w > 1/2 + fRoot = Math::Sqrt(fTrace + (Real)1.0); // 2w + m_afTuple[0] = ((Real)0.5)*fRoot; + fRoot = ((Real)0.5)/fRoot; // 1/(4w) + m_afTuple[1] = (rkRot(2,1)-rkRot(1,2))*fRoot; + m_afTuple[2] = (rkRot(0,2)-rkRot(2,0))*fRoot; + m_afTuple[3] = (rkRot(1,0)-rkRot(0,1))*fRoot; + } + else + { + // |w| <= 1/2 + int i = 0; + if ( rkRot(1,1) > rkRot(0,0) ) + { + i = 1; + } + if ( rkRot(2,2) > rkRot(i,i) ) + { + i = 2; + } + int j = ms_iNext[i]; + int k = ms_iNext[j]; + + fRoot = Math::Sqrt(rkRot(i,i)-rkRot(j,j)-rkRot(k,k)+(Real)1.0); + Real* apfQuat[3] = { &m_afTuple[1], &m_afTuple[2], &m_afTuple[3] }; + *apfQuat[i] = ((Real)0.5)*fRoot; + fRoot = ((Real)0.5)/fRoot; + m_afTuple[0] = (rkRot(k,j)-rkRot(j,k))*fRoot; + *apfQuat[j] = (rkRot(j,i)+rkRot(i,j))*fRoot; + *apfQuat[k] = (rkRot(k,i)+rkRot(i,k))*fRoot; + } + + return *this; +} +//---------------------------------------------------------------------------- +template +void Quaternion::ToRotationMatrix (Matrix3& rkRot) const +{ + Real fTx = ((Real)2.0)*m_afTuple[1]; + Real fTy = ((Real)2.0)*m_afTuple[2]; + Real fTz = ((Real)2.0)*m_afTuple[3]; + Real fTwx = fTx*m_afTuple[0]; + Real fTwy = fTy*m_afTuple[0]; + Real fTwz = fTz*m_afTuple[0]; + Real fTxx = fTx*m_afTuple[1]; + Real fTxy = fTy*m_afTuple[1]; + Real fTxz = fTz*m_afTuple[1]; + Real fTyy = fTy*m_afTuple[2]; + Real fTyz = fTz*m_afTuple[2]; + Real fTzz = fTz*m_afTuple[3]; + + rkRot(0,0) = (Real)1.0-(fTyy+fTzz); + rkRot(0,1) = fTxy-fTwz; + rkRot(0,2) = fTxz+fTwy; + rkRot(1,0) = fTxy+fTwz; + rkRot(1,1) = (Real)1.0-(fTxx+fTzz); + rkRot(1,2) = fTyz-fTwx; + rkRot(2,0) = fTxz-fTwy; + rkRot(2,1) = fTyz+fTwx; + rkRot(2,2) = (Real)1.0-(fTxx+fTyy); +} +//---------------------------------------------------------------------------- +template +Quaternion& Quaternion::FromRotationMatrix ( + const Vector3 akRotColumn[3]) +{ + Matrix3 kRot; + for (int iCol = 0; iCol < 3; iCol++) + { + kRot(0,iCol) = akRotColumn[iCol][0]; + kRot(1,iCol) = akRotColumn[iCol][1]; + kRot(2,iCol) = akRotColumn[iCol][2]; + } + return FromRotationMatrix(kRot); +} +//---------------------------------------------------------------------------- +template +void Quaternion::ToRotationMatrix (Vector3 akRotColumn[3]) const +{ + Matrix3 kRot; + ToRotationMatrix(kRot); + for (int iCol = 0; iCol < 3; iCol++) + { + akRotColumn[iCol][0] = kRot(0,iCol); + akRotColumn[iCol][1] = kRot(1,iCol); + akRotColumn[iCol][2] = kRot(2,iCol); + } +} +//---------------------------------------------------------------------------- +template +Quaternion& Quaternion::FromAxisAngle ( + const Vector3& rkAxis, Real fAngle) +{ + // assert: axis[] is unit length + // + // The quaternion representing the rotation is + // q = cos(A/2)+sin(A/2)*(x*i+y*j+z*k) + + Real fHalfAngle = ((Real)0.5)*fAngle; + Real fSin = Math::Sin(fHalfAngle); + m_afTuple[0] = Math::Cos(fHalfAngle); + m_afTuple[1] = fSin*rkAxis[0]; + m_afTuple[2] = fSin*rkAxis[1]; + m_afTuple[3] = fSin*rkAxis[2]; + + return *this; +} +//---------------------------------------------------------------------------- +template +void Quaternion::ToAxisAngle (Vector3& rkAxis, Real& rfAngle) + const +{ + // The quaternion representing the rotation is + // q = cos(A/2)+sin(A/2)*(x*i+y*j+z*k) + + Real fSqrLength = m_afTuple[1]*m_afTuple[1] + m_afTuple[2]*m_afTuple[2] + + m_afTuple[3]*m_afTuple[3]; + + if (fSqrLength > Math::ZERO_TOLERANCE) + { + rfAngle = ((Real)2.0)*Math::ACos(m_afTuple[0]); + Real fInvLength = Math::InvSqrt(fSqrLength); + rkAxis[0] = m_afTuple[1]*fInvLength; + rkAxis[1] = m_afTuple[2]*fInvLength; + rkAxis[2] = m_afTuple[3]*fInvLength; + } + else + { + // angle is 0 (mod 2*pi), so any axis will do + rfAngle = (Real)0.0; + rkAxis[0] = (Real)1.0; + rkAxis[1] = (Real)0.0; + rkAxis[2] = (Real)0.0; + } +} +//---------------------------------------------------------------------------- +template +inline Real Quaternion::Length () const +{ + return Math::Sqrt( + m_afTuple[0]*m_afTuple[0] + + m_afTuple[1]*m_afTuple[1] + + m_afTuple[2]*m_afTuple[2] + + m_afTuple[3]*m_afTuple[3]); +} +//---------------------------------------------------------------------------- +template +inline Real Quaternion::SquaredLength () const +{ + return + m_afTuple[0]*m_afTuple[0] + + m_afTuple[1]*m_afTuple[1] + + m_afTuple[2]*m_afTuple[2] + + m_afTuple[3]*m_afTuple[3]; +} +//---------------------------------------------------------------------------- +template +inline Real Quaternion::Dot (const Quaternion& rkQ) const +{ + Real fDot = (Real)0.0; + for (int i = 0; i < 4; i++) + { + fDot += m_afTuple[i]*rkQ.m_afTuple[i]; + } + return fDot; +} +//---------------------------------------------------------------------------- +template +inline Real Quaternion::Normalize () +{ + Real fLength = Length(); + + if (fLength > Math::ZERO_TOLERANCE) + { + Real fInvLength = ((Real)1.0)/fLength; + m_afTuple[0] *= fInvLength; + m_afTuple[1] *= fInvLength; + m_afTuple[2] *= fInvLength; + m_afTuple[3] *= fInvLength; + } + else + { + fLength = (Real)0.0; + m_afTuple[0] = (Real)0.0; + m_afTuple[1] = (Real)0.0; + m_afTuple[2] = (Real)0.0; + m_afTuple[3] = (Real)0.0; + } + + return fLength; +} +//---------------------------------------------------------------------------- +template +Quaternion Quaternion::Inverse () const +{ + Quaternion kInverse; + + Real fNorm = (Real)0.0; + int i; + for (i = 0; i < 4; i++) + { + fNorm += m_afTuple[i]*m_afTuple[i]; + } + + if (fNorm > (Real)0.0) + { + Real fInvNorm = ((Real)1.0)/fNorm; + kInverse.m_afTuple[0] = m_afTuple[0]*fInvNorm; + kInverse.m_afTuple[1] = -m_afTuple[1]*fInvNorm; + kInverse.m_afTuple[2] = -m_afTuple[2]*fInvNorm; + kInverse.m_afTuple[3] = -m_afTuple[3]*fInvNorm; + } + else + { + // return an invalid result to flag the error + for (i = 0; i < 4; i++) + { + kInverse.m_afTuple[i] = (Real)0.0; + } + } + + return kInverse; +} +//---------------------------------------------------------------------------- +template +Quaternion Quaternion::Conjugate () const +{ + return Quaternion(m_afTuple[0],-m_afTuple[1],-m_afTuple[2], + -m_afTuple[3]); +} +//---------------------------------------------------------------------------- +template +Quaternion Quaternion::Exp () const +{ + // If q = A*(x*i+y*j+z*k) where (x,y,z) is unit length, then + // exp(q) = cos(A)+sin(A)*(x*i+y*j+z*k). If sin(A) is near zero, + // use exp(q) = cos(A)+A*(x*i+y*j+z*k) since A/sin(A) has limit 1. + + Quaternion kResult; + + Real fAngle = Math::Sqrt(m_afTuple[1]*m_afTuple[1] + + m_afTuple[2]*m_afTuple[2] + m_afTuple[3]*m_afTuple[3]); + + Real fSin = Math::Sin(fAngle); + kResult.m_afTuple[0] = Math::Cos(fAngle); + + int i; + + if (Math::FAbs(fSin) >= Math::ZERO_TOLERANCE) + { + Real fCoeff = fSin/fAngle; + for (i = 1; i <= 3; i++) + { + kResult.m_afTuple[i] = fCoeff*m_afTuple[i]; + } + } + else + { + for (i = 1; i <= 3; i++) + { + kResult.m_afTuple[i] = m_afTuple[i]; + } + } + + return kResult; +} +//---------------------------------------------------------------------------- +template +Quaternion Quaternion::Log () const +{ + // If q = cos(A)+sin(A)*(x*i+y*j+z*k) where (x,y,z) is unit length, then + // log(q) = A*(x*i+y*j+z*k). If sin(A) is near zero, use log(q) = + // sin(A)*(x*i+y*j+z*k) since sin(A)/A has limit 1. + + Quaternion kResult; + kResult.m_afTuple[0] = (Real)0.0; + + int i; + + if (Math::FAbs(m_afTuple[0]) < (Real)1.0) + { + Real fAngle = Math::ACos(m_afTuple[0]); + Real fSin = Math::Sin(fAngle); + if (Math::FAbs(fSin) >= Math::ZERO_TOLERANCE) + { + Real fCoeff = fAngle/fSin; + for (i = 1; i <= 3; i++) + { + kResult.m_afTuple[i] = fCoeff*m_afTuple[i]; + } + return kResult; + } + } + + for (i = 1; i <= 3; i++) + { + kResult.m_afTuple[i] = m_afTuple[i]; + } + return kResult; +} +//---------------------------------------------------------------------------- +template +Vector3 Quaternion::Rotate (const Vector3& rkVector) + const +{ + // Given a vector u = (x0,y0,z0) and a unit length quaternion + // q = , the vector v = (x1,y1,z1) which represents the + // rotation of u by q is v = q*u*q^{-1} where * indicates quaternion + // multiplication and where u is treated as the quaternion <0,x0,y0,z0>. + // Note that q^{-1} = , so no real work is required to + // invert q. Now + // + // q*u*q^{-1} = q*<0,x0,y0,z0>*q^{-1} + // = q*(x0*i+y0*j+z0*k)*q^{-1} + // = x0*(q*i*q^{-1})+y0*(q*j*q^{-1})+z0*(q*k*q^{-1}) + // + // As 3-vectors, q*i*q^{-1}, q*j*q^{-1}, and 2*k*q^{-1} are the columns + // of the rotation matrix computed in Quaternion::ToRotationMatrix. + // The vector v is obtained as the product of that rotation matrix with + // vector u. As such, the quaternion representation of a rotation + // matrix requires less space than the matrix and more time to compute + // the rotated vector. Typical space-time tradeoff... + + Matrix3 kRot; + ToRotationMatrix(kRot); + return kRot*rkVector; +} +//---------------------------------------------------------------------------- +template +Quaternion& Quaternion::Slerp (Real fT, const Quaternion& rkP, + const Quaternion& rkQ) +{ + Real fCos = rkP.Dot(rkQ); + Real fAngle = Math::ACos(fCos); + + if (Math::FAbs(fAngle) >= Math::ZERO_TOLERANCE) + { + Real fSin = Math::Sin(fAngle); + Real fInvSin = ((Real)1.0)/fSin; + Real fTAngle = fT*fAngle; + Real fCoeff0 = Math::Sin(fAngle - fTAngle)*fInvSin; + Real fCoeff1 = Math::Sin(fTAngle)*fInvSin; + + // Profiling showed that the old line of code, + // *this = fCoeff0*rkP + fCoeff1*rkQ; + // was using a large number of cycles, more so than the sin and cos + // function calls. The following inlined code is much faster. + m_afTuple[0] = fCoeff0*rkP.m_afTuple[0] + fCoeff1*rkQ.m_afTuple[0]; + m_afTuple[1] = fCoeff0*rkP.m_afTuple[1] + fCoeff1*rkQ.m_afTuple[1]; + m_afTuple[2] = fCoeff0*rkP.m_afTuple[2] + fCoeff1*rkQ.m_afTuple[2]; + m_afTuple[3] = fCoeff0*rkP.m_afTuple[3] + fCoeff1*rkQ.m_afTuple[3]; + } + else + { + // Based on the problem with the code in the previous block, inlining + // the old code + // *this = rkP; + m_afTuple[0] = rkP.m_afTuple[0]; + m_afTuple[1] = rkP.m_afTuple[1]; + m_afTuple[2] = rkP.m_afTuple[2]; + m_afTuple[3] = rkP.m_afTuple[3]; + } + + return *this; +} +//---------------------------------------------------------------------------- +template +Quaternion& Quaternion::SlerpExtraSpins (Real fT, + const Quaternion& rkP, const Quaternion& rkQ, int iExtraSpins) +{ + Real fCos = rkP.Dot(rkQ); + Real fAngle = Math::ACos(fCos); + + if (Math::FAbs(fAngle) >= Math::ZERO_TOLERANCE) + { + Real fSin = Math::Sin(fAngle); + Real fPhase = Math::PI*iExtraSpins*fT; + Real fInvSin = ((Real)1.0)/fSin; + Real fCoeff0 = Math::Sin(((Real)1.0-fT)*fAngle-fPhase)*fInvSin; + Real fCoeff1 = Math::Sin(fT*fAngle + fPhase)*fInvSin; + *this = fCoeff0*rkP + fCoeff1*rkQ; + } + else + { + *this = rkP; + } + + return *this; +} +//---------------------------------------------------------------------------- +template +Quaternion& Quaternion::Intermediate (const Quaternion& rkQ0, + const Quaternion& rkQ1, const Quaternion& rkQ2) +{ + // assert: Q0, Q1, Q2 all unit-length + Quaternion kQ1Inv = rkQ1.Conjugate(); + Quaternion kP0 = kQ1Inv*rkQ0; + Quaternion kP2 = kQ1Inv*rkQ2; + Quaternion kArg = -((Real)0.25)*(kP0.Log()+kP2.Log()); + Quaternion kA = rkQ1*kArg.Exp(); + *this = kA; + + return *this; +} +//---------------------------------------------------------------------------- +template +Quaternion& Quaternion::Squad (Real fT, const Quaternion& rkQ0, + const Quaternion& rkA0, const Quaternion& rkA1, const Quaternion& rkQ1) +{ + Real fSlerpT = ((Real)2.0)*fT*((Real)1.0-fT); + Quaternion kSlerpP = Slerp(fT,rkQ0,rkQ1); + Quaternion kSlerpQ = Slerp(fT,rkA0,rkA1); + return Slerp(fSlerpT,kSlerpP,kSlerpQ); +} +//---------------------------------------------------------------------------- +template +Quaternion& Quaternion::Align (const Vector3& rkV1, + const Vector3& rkV2) +{ + // If V1 and V2 are not parallel, the axis of rotation is the unit-length + // vector U = Cross(V1,V2)/Length(Cross(V1,V2)). The angle of rotation, + // A, is the angle between V1 and V2. The quaternion for the rotation is + // q = cos(A/2) + sin(A/2)*(ux*i+uy*j+uz*k) where U = (ux,uy,uz). + // + // (1) Rather than extract A = acos(Dot(V1,V2)), multiply by 1/2, then + // compute sin(A/2) and cos(A/2), we reduce the computational costs by + // computing the bisector B = (V1+V2)/Length(V1+V2), so cos(A/2) = + // Dot(V1,B). + // + // (2) The rotation axis is U = Cross(V1,B)/Length(Cross(V1,B)), but + // Length(Cross(V1,B)) = Length(V1)*Length(B)*sin(A/2) = sin(A/2), in + // which case sin(A/2)*(ux*i+uy*j+uz*k) = (cx*i+cy*j+cz*k) where + // C = Cross(V1,B). + // + // If V1 = V2, then B = V1, cos(A/2) = 1, and U = (0,0,0). If V1 = -V2, + // then B = 0. This can happen even if V1 is approximately -V2 using + // floating point arithmetic, since Vector3::Normalize checks for + // closeness to zero and returns the zero vector accordingly. The test + // for exactly zero is usually not recommend for floating point + // arithmetic, but the implementation of Vector3::Normalize guarantees + // the comparison is robust. In this case, the A = pi and any axis + // perpendicular to V1 may be used as the rotation axis. + + Vector3 kBisector = rkV1 + rkV2; + kBisector.Normalize(); + + Real fCosHalfAngle = rkV1.Dot(kBisector); + Vector3 kCross; + + m_afTuple[0] = fCosHalfAngle; + + if (fCosHalfAngle != (Real)0.0) + { + kCross = rkV1.Cross(kBisector); + m_afTuple[1] = kCross.X(); + m_afTuple[2] = kCross.Y(); + m_afTuple[3] = kCross.Z(); + } + else + { + Real fInvLength; + if (Math::FAbs(rkV1[0]) >= Math::FAbs(rkV1[1])) + { + // V1.x or V1.z is the largest magnitude component + fInvLength = Math::InvSqrt(rkV1[0]*rkV1[0] + + rkV1[2]*rkV1[2]); + + m_afTuple[1] = -rkV1[2]*fInvLength; + m_afTuple[2] = (Real)0.0; + m_afTuple[3] = +rkV1[0]*fInvLength; + } + else + { + // V1.y or V1.z is the largest magnitude component + fInvLength = Math::InvSqrt(rkV1[1]*rkV1[1] + + rkV1[2]*rkV1[2]); + + m_afTuple[1] = (Real)0.0; + m_afTuple[2] = +rkV1[2]*fInvLength; + m_afTuple[3] = -rkV1[1]*fInvLength; + } + } + + return *this; +} +//---------------------------------------------------------------------------- +template +void Quaternion::DecomposeTwistTimesSwing ( + const Vector3& rkV1, Quaternion& rkTwist, Quaternion& rkSwing) +{ + Vector3 kV2 = Rotate(rkV1); + rkSwing = Align(rkV1,kV2); + rkTwist = (*this)*rkSwing.Conjugate(); +} +//---------------------------------------------------------------------------- +template +void Quaternion::DecomposeSwingTimesTwist ( + const Vector3& rkV1, Quaternion& rkSwing, Quaternion& rkTwist) +{ + Vector3 kV2 = Rotate(rkV1); + rkSwing = Align(rkV1,kV2); + rkTwist = rkSwing.Conjugate()*(*this); +} +//---------------------------------------------------------------------------- +template +Quaternion Quaternion::GetClosest (int iAxis) const +{ + // The appropriate nonzero components will be set later. + Quaternion kQ((Real)0,(Real)0,(Real)0,(Real)0); + Real fP0 = m_afTuple[0]; + Real fP1 = m_afTuple[iAxis]; + Real fSqrLength = fP0*fP0 + fP1*fP1; + if (fSqrLength > ms_fTolerance) + { + // A unique closest point. + Real fInvLength = Math::InvSqrt(fSqrLength); + kQ[0] = fP0*fInvLength; + kQ[iAxis] = fP1*fInvLength; + } + else + { + // Infinitely many solutions, choose the one for theta = 0. + kQ[0] = (Real)1; + kQ[iAxis] = (Real)0; + } + return kQ; +} +//---------------------------------------------------------------------------- +template +Quaternion Quaternion::GetClosestX () const +{ + return GetClosest(1); +} +//---------------------------------------------------------------------------- +template +Quaternion Quaternion::GetClosestY () const +{ + return GetClosest(2); +} +//---------------------------------------------------------------------------- +template +Quaternion Quaternion::GetClosestZ () const +{ + return GetClosest(3); +} +//---------------------------------------------------------------------------- +template +Quaternion Quaternion::GetClosestXY () const +{ + Quaternion kQ; + + Real fDet = m_afTuple[0]*m_afTuple[3] - m_afTuple[1]*m_afTuple[2]; + if(Math::FAbs(fDet) < (Real)0.5 - ms_fTolerance) + { + Real fDiscr = (Real)1 - ((Real)4)*fDet*fDet; + fDiscr = Math::Sqrt(Math::FAbs(fDiscr)); + Real fA = m_afTuple[0]*m_afTuple[1] + m_afTuple[2]*m_afTuple[3]; + Real fB = m_afTuple[0]*m_afTuple[0] - m_afTuple[1]*m_afTuple[1] + + m_afTuple[2]*m_afTuple[2] - m_afTuple[3]*m_afTuple[3]; + + Real fC0, fS0, fC1, fS1, fInvLength; + + if (fB >= (Real)0) + { + fC0 = ((Real)0.5)*(fDiscr + fB); + fS0 = fA; + } + else + { + fC0 = fA; + fS0 = ((Real)0.5)*(fDiscr - fB); + } + fInvLength = Math::InvSqrt(fC0*fC0 + fS0*fS0); + fC0 *= fInvLength; + fS0 *= fInvLength; + + fC1 = m_afTuple[0]*fC0 + m_afTuple[1]*fS0; + fS1 = m_afTuple[2]*fC0 + m_afTuple[3]*fS0; + fInvLength = Math::InvSqrt(fC1*fC1 + fS1*fS1); + fC1 *= fInvLength; + fS1 *= fInvLength; + + kQ[0] = fC0*fC1; + kQ[1] = fS0*fC1; + kQ[2] = fC0*fS1; + kQ[3] = fS0*fS1; + } + else + { + Real fInvLength = Math::InvSqrt(Math::FAbs(fDet)); + kQ[0] = m_afTuple[0]*fInvLength; + kQ[1] = m_afTuple[1]*fInvLength; + kQ[2] = (Real)0; + kQ[3] = (Real)0; + } + + return kQ; +} +//---------------------------------------------------------------------------- +template +Quaternion Quaternion::GetClosestYX () const +{ + Quaternion kAlt(m_afTuple[0],m_afTuple[1],m_afTuple[2],-m_afTuple[3]); + Quaternion kQ = kAlt.GetClosestXY(); + kQ[3] = -kQ[3]; + return kQ; +} +//---------------------------------------------------------------------------- +template +Quaternion Quaternion::GetClosestZX () const +{ + Quaternion kAlt(m_afTuple[0],m_afTuple[1],m_afTuple[3],m_afTuple[2]); + Quaternion kQ = kAlt.GetClosestXY(); + Real fSave = kQ[2]; + kQ[2] = kQ[3]; + kQ[3] = fSave; + return kQ; +} +//---------------------------------------------------------------------------- +template +Quaternion Quaternion::GetClosestXZ () const +{ + Quaternion kAlt(m_afTuple[0],m_afTuple[1],-m_afTuple[3],m_afTuple[2]); + Quaternion kQ = kAlt.GetClosestXY(); + Real fSave = kQ[2]; + kQ[2] = kQ[3]; + kQ[3] = -fSave; + return kQ; +} +//---------------------------------------------------------------------------- +template +Quaternion Quaternion::GetClosestYZ () const +{ + Quaternion kAlt(m_afTuple[0],m_afTuple[2],m_afTuple[3],m_afTuple[1]); + Quaternion kQ = kAlt.GetClosestXY(); + Real fSave = kQ[3]; + kQ[3] = kQ[2]; + kQ[2] = kQ[1]; + kQ[1] = fSave; + return kQ; +} +//---------------------------------------------------------------------------- +template +Quaternion Quaternion::GetClosestZY () const +{ + Quaternion kAlt(m_afTuple[0],m_afTuple[2],m_afTuple[3],-m_afTuple[1]); + Quaternion kQ = kAlt.GetClosestXY(); + Real fSave = kQ[3]; + kQ[3] = kQ[2]; + kQ[2] = kQ[1]; + kQ[1] = -fSave; + return kQ; +} +//---------------------------------------------------------------------------- +template +void Quaternion::FactorXYZ (Real& rfCx, Real& rfSx, Real& rfCy, + Real& rfSy, Real& rfCz, Real& rfSz) +{ + Real fA = m_afTuple[0]*m_afTuple[1] - m_afTuple[2]*m_afTuple[3]; + Real fB = ((Real)0.5)*( + m_afTuple[0]*m_afTuple[0] + - m_afTuple[1]*m_afTuple[1] + - m_afTuple[2]*m_afTuple[2] + + m_afTuple[3]*m_afTuple[3]); + + Real fLength = Math::Sqrt(fA*fA + fB*fB); + if (fLength > ms_fTolerance) + { + Real fInvLength = ((Real)1)/fLength; + Real fSigma0 = fA * fInvLength; + Real fGamma0 = fB * fInvLength; + if (fGamma0 >= (Real)0) + { + rfCx = Math::Sqrt(((Real)0.5)*((Real)1 + fGamma0)); + rfSx = ((Real)0.5)*fSigma0/rfCx; + } + else + { + rfSx = Math::Sqrt(((Real)0.5)*((Real)1 - fGamma0)); + rfCx = ((Real)0.5)*fSigma0/rfSx; + } + + Real fTmp0 = rfCx*m_afTuple[0] + rfSx*m_afTuple[1]; + Real fTmp1 = rfCx*m_afTuple[3] - rfSx*m_afTuple[2]; + fInvLength = Math::InvSqrt(fTmp0*fTmp0 + fTmp1*fTmp1); + rfCz = fTmp0 * fInvLength; + rfSz = fTmp1 * fInvLength; + + if(Math::FAbs(rfCz) >= Math::FAbs(rfSz)) + { + fInvLength = ((Real)1)/rfCz; + rfCy = fTmp0 * fInvLength; + rfSy = (rfCx*m_afTuple[2] + rfSx*m_afTuple[3]) * fInvLength; + } + else + { + fInvLength = ((Real)1)/rfSz; + rfCy = fTmp1 * fInvLength; + rfSy = (rfCx*m_afTuple[1] - rfSx*m_afTuple[0]) * fInvLength; + } + } + else + { + // Infinitely many solutions. Choose one of them. + if(m_afTuple[0]*m_afTuple[2] + m_afTuple[1]*m_afTuple[3] > (Real)0) + { + // p = (p0,p1,p0,p1) + rfCx = (Real)1; + rfSx = (Real)0; + rfCy = ms_fRootHalf; + rfSy = ms_fRootHalf; + rfCz = ms_fRootTwo * m_afTuple[0]; + rfSz = ms_fRootTwo * m_afTuple[1]; + } + else + { + // p = (p0,p1,-p0,-p1) + rfCx = (Real)1; + rfSx = (Real)0; + rfCy = ms_fRootHalf; + rfSy = -ms_fRootHalf; + rfCz = ms_fRootTwo * m_afTuple[0]; + rfSz = -ms_fRootTwo * m_afTuple[1]; + } + } +} +//---------------------------------------------------------------------------- +template +void Quaternion::FactorXZY (Real& rfCx, Real& rfSx, Real& rfCz, + Real& rfSz, Real& rfCy, Real& rfSy) +{ + Quaternion pkAlt(m_afTuple[0],m_afTuple[1],m_afTuple[3],-m_afTuple[2]); + pkAlt.FactorXYZ(rfCx,rfSx,rfCz,rfSz,rfCy,rfSy); + rfSy = -rfSy; +} +//---------------------------------------------------------------------------- +template +void Quaternion::FactorYZX (Real& rfCy, Real& rfSy, Real& rfCz, + Real& rfSz, Real& rfCx, Real& rfSx) +{ + Quaternion pkAlt(m_afTuple[0],-m_afTuple[2],m_afTuple[3],-m_afTuple[1]); + pkAlt.FactorXYZ(rfCy,rfSy,rfCz,rfSz,rfCx,rfSx); + rfSx = -rfSx; + rfSy = -rfSy; +} +//---------------------------------------------------------------------------- +template +void Quaternion::FactorYXZ (Real& rfCy, Real& rfSy, Real& rfCx, + Real& rfSx, Real& rfCz, Real& rfSz) +{ + Quaternion pkAlt(m_afTuple[0],-m_afTuple[2],m_afTuple[1],m_afTuple[3]); + pkAlt.FactorXYZ(rfCy,rfSy,rfCx,rfSx,rfCz,rfSz); + rfSy = -rfSy; +} +//---------------------------------------------------------------------------- +template +void Quaternion::FactorZXY (Real& rfCz, Real& rfSz, Real& rfCx, + Real& rfSx, Real& rfCy, Real& rfSy) +{ + Quaternion pkAlt(m_afTuple[0],-m_afTuple[3],m_afTuple[1],-m_afTuple[2]); + pkAlt.FactorXYZ(rfCz,rfSz,rfCx,rfSx,rfCy,rfSy); + rfSy = -rfSy; + rfSz = -rfSz; +} +//---------------------------------------------------------------------------- +template +void Quaternion::FactorZYX (Real& rfCz, Real& rfSz, Real& rfCy, + Real& rfSy, Real& rfCx, Real& rfSx) +{ + Quaternion pkAlt(m_afTuple[0],m_afTuple[3],-m_afTuple[2],m_afTuple[1]); + pkAlt.FactorXYZ(rfCz,rfSz,rfCy,rfSy,rfCx,rfSx); + rfSy = -rfSy; +} +//---------------------------------------------------------------------------- +template +Quaternion Quaternion::GetClosest (int iAxis, + const Constraints& rkCon) const +{ + Quaternion kQ((Real)0,(Real)0,(Real)0,(Real)0); + + Real fP0 = m_afTuple[0]; + Real fP1 = m_afTuple[iAxis]; + Real fSqrLength = fP0*fP0 + fP1*fP1; + if (fSqrLength > ms_fTolerance) + { + Real fInvLength = Math::InvSqrt(fSqrLength); + fP0 *= fInvLength; + fP1 *= fInvLength; + if (rkCon.IsValid(fP0,fP1)) + { + // The maximum occurs at an interior point. + kQ[0] = fP0; + kQ[iAxis] = fP1; + } + else + { + // The maximum occurs at a boundary point. + Real fCsMin = rkCon.m_fCosMinAngle; + Real fSnMin = rkCon.m_fSinMinAngle; + Real fDotMinAngle = fP0*fCsMin + fP1*fSnMin; + if (fDotMinAngle < (Real)0) + { + fCsMin = -fCsMin; + fSnMin = -fSnMin; + fDotMinAngle = -fDotMinAngle; + } + + Real fCsMax = rkCon.m_fCosMaxAngle; + Real fSnMax = rkCon.m_fSinMaxAngle; + Real fDotMaxAngle = fP0*fCsMax + fP1*fSnMax; + if (fDotMaxAngle < (Real)0) + { + fCsMax = -fCsMax; + fSnMax = -fSnMax; + fDotMaxAngle = -fDotMaxAngle; + } + + if (fDotMaxAngle >= fDotMinAngle) + { + kQ[0] = fCsMax; + kQ[iAxis] = fSnMax; + } + else + { + kQ[0] = fCsMin; + kQ[iAxis] = fSnMin; + } + } + } + else + { + // Infinitely many solutions, choose one that satisfies the angle + // constraints. + kQ[0] = rkCon.m_fCosAvrAngle; + kQ[iAxis] = rkCon.m_fSinAvrAngle; + } + + return kQ; +} +//---------------------------------------------------------------------------- +template +Quaternion Quaternion::GetClosestX (const Constraints& rkXCon) + const +{ + return GetClosest(1,rkXCon); +} +//---------------------------------------------------------------------------- +template +Quaternion Quaternion::GetClosestY (const Constraints& rkYCon) + const +{ + return GetClosest(2,rkYCon); +} +//---------------------------------------------------------------------------- +template +Quaternion Quaternion::GetClosestZ (const Constraints& rkZCon) + const +{ + return GetClosest(3,rkZCon); +} +//---------------------------------------------------------------------------- +template +Quaternion Quaternion::GetClosestXY (const Constraints& rkXCon, + const Constraints& rkYCon) const +{ + Quaternion kQ, kTmp; + Real fC0, fS0, fC1, fS1, fInvLength; + + Real fDet = m_afTuple[0]*m_afTuple[3] - m_afTuple[1]*m_afTuple[2]; + if (Math::FAbs(fDet) < (Real)0.5 - ms_fTolerance) + { + Real fDiscr = Math::Sqrt(Math::FAbs((Real)1 - + ((Real)4)*fDet*fDet)); + Real fA = m_afTuple[0]*m_afTuple[1] + m_afTuple[2]*m_afTuple[3]; + Real fB = m_afTuple[0]*m_afTuple[0] - m_afTuple[1]*m_afTuple[1] + + m_afTuple[2]*m_afTuple[2] - m_afTuple[3]*m_afTuple[3]; + + if (fB >= (Real)0) + { + fC0 = ((Real)0.5)*(fDiscr + fB); + fS0 = fA; + } + else + { + fC0 = fA; + fS0 = ((Real)0.5)*(fDiscr - fB); + } + fInvLength = Math::InvSqrt(fC0*fC0 + fS0*fS0); + fC0 *= fInvLength; + fS0 *= fInvLength; + + fC1 = m_afTuple[0]*fC0 + m_afTuple[1]*fS0; + fS1 = m_afTuple[2]*fC0 + m_afTuple[3]*fS0; + fInvLength = Math::InvSqrt(fC1*fC1 + fS1*fS1); + fC1 *= fInvLength; + fS1 *= fInvLength; + + if (rkXCon.IsValid(fC0,fS0) && rkYCon.IsValid(fC1,fS1)) + { + // The maximum occurs at an interior point. + kQ[0] = fC0*fC1; + kQ[1] = fS0*fC1; + kQ[2] = fC0*fS1; + kQ[3] = fS0*fS1; + } + else + { + // The maximum occurs at a boundary point. + Quaternion kR(rkXCon.m_fCosMinAngle,rkXCon.m_fSinMinAngle, + (Real)0,(Real)0); + Quaternion kRInv(rkXCon.m_fCosMinAngle,-rkXCon.m_fSinMinAngle, + (Real)0,(Real)0); + Quaternion kProd = kRInv*(*this); + kTmp = kProd.GetClosest(2,rkYCon); + Real fDotOptAngle = kProd.Dot(kTmp); + kQ = kR*kTmp; + + kR = Quaternion(rkXCon.m_fCosMaxAngle,rkXCon.m_fSinMaxAngle, + (Real)0,(Real)0); + kRInv = Quaternion(rkXCon.m_fCosMaxAngle,-rkXCon.m_fSinMaxAngle, + (Real)0,(Real)0); + kProd = kRInv*(*this); + kTmp = kProd.GetClosest(2,rkYCon); + Real fDotAngle = kProd.Dot(kTmp); + if (fDotAngle > fDotOptAngle) + { + kQ = kR*kTmp; + fDotOptAngle = fDotAngle; + } + + kR = Quaternion(rkYCon.m_fCosMinAngle,(Real)0, + rkYCon.m_fSinMinAngle,(Real)0); + kRInv = Quaternion(rkYCon.m_fCosMinAngle,(Real)0, + -rkYCon.m_fSinMinAngle,(Real)0); + kProd = (*this)*kRInv; + kTmp = kProd.GetClosest(1,rkXCon); + fDotAngle = kProd.Dot(kTmp); + if (fDotAngle > fDotOptAngle) + { + kQ = kTmp*kR; + fDotOptAngle = fDotAngle; + } + + kR = Quaternion(rkYCon.m_fCosMaxAngle,(Real)0, + rkYCon.m_fSinMaxAngle,(Real)0); + kRInv = Quaternion(rkYCon.m_fCosMaxAngle,(Real)0, + -rkYCon.m_fSinMaxAngle,(Real)0); + kProd = (*this)*kRInv; + kTmp = kProd.GetClosest(1,rkXCon); + fDotAngle = kProd.Dot(kTmp); + if (fDotAngle > fDotOptAngle) + { + kQ = kTmp*kR; + fDotOptAngle = fDotAngle; + } + } + } + else + { + // Infinitely many solutions, choose one that satisfies the angle + // constraints. + Real fMinAngle, fMaxAngle, fAngle; + Constraints kCon; + + if (fDet > (Real)0) + { + fMinAngle = rkXCon.m_fMinAngle - rkYCon.m_fMaxAngle; + fMaxAngle = rkXCon.m_fMaxAngle - rkYCon.m_fMinAngle; + kCon.SetAngles(fMinAngle,fMaxAngle); + + kTmp = GetClosest(1,kCon); + + fAngle = Math::ATan2(kTmp[1],kTmp[0]); + if (fAngle < fMinAngle || fAngle > fMaxAngle) + { + fAngle -= + (kTmp[1] >= (Real)0 ? Math::PI : -Math::PI); + // assert(fMinAngle <= fAngle && fAngle <= fMaxAngle); + } + + if (fAngle <= rkXCon.m_fMaxAngle - rkYCon.m_fMaxAngle) + { + fC1 = rkYCon.m_fCosMaxAngle; + fS1 = rkYCon.m_fSinMaxAngle; + fAngle = rkYCon.m_fMaxAngle + fAngle; + fC0 = Math::Cos(fAngle); + fS0 = Math::Sin(fAngle); + } + else + { + fC0 = rkXCon.m_fCosMaxAngle; + fS0 = rkXCon.m_fSinMaxAngle; + fAngle = rkXCon.m_fMaxAngle - fAngle; + fC1 = Math::Cos(fAngle); + fS1 = Math::Sin(fAngle); + } + } + else + { + fMinAngle = rkXCon.m_fMinAngle + rkYCon.m_fMinAngle; + fMaxAngle = rkXCon.m_fMaxAngle + rkYCon.m_fMaxAngle; + kCon.SetAngles(fMinAngle,fMaxAngle); + + kTmp = GetClosest(1,kCon); + + fAngle = Math::ATan2(kTmp[1],kTmp[0]); + if (fAngle < fMinAngle || fAngle > fMaxAngle) + { + fAngle -= + (kTmp[1] >= (Real)0 ? Math::PI : -Math::PI); + // assert(fMinAngle <= fAngle && fAngle <= fMaxAngle); + } + + if (fAngle >= rkXCon.m_fMinAngle + rkYCon.m_fMaxAngle) + { + fC1 = rkYCon.m_fCosMaxAngle; + fS1 = rkYCon.m_fSinMaxAngle; + fAngle = fAngle - rkYCon.m_fMaxAngle; + fC0 = Math::Cos(fAngle); + fS0 = Math::Sin(fAngle); + } + else + { + fC0 = rkXCon.m_fCosMaxAngle; + fS0 = rkXCon.m_fSinMaxAngle; + fAngle = fAngle - rkXCon.m_fMaxAngle; + fC1 = Math::Cos(fAngle); + fS1 = Math::Sin(fAngle); + } + } + + kQ[0] = fC0*fC1; + kQ[1] = fS0*fC1; + kQ[2] = fC0*fS1; + kQ[3] = fS0*fS1; + if (Dot(kQ) < (Real)0) + { + kQ = -kQ; + } + } + + return kQ; +} +//---------------------------------------------------------------------------- +template +Quaternion Quaternion::GetClosestYX (const Constraints& rkYCon, + const Constraints& rkXCon) const +{ + Quaternion pkAlt(m_afTuple[0],m_afTuple[1],m_afTuple[2],-m_afTuple[3]); + Quaternion kQ = pkAlt.GetClosestXY(rkXCon,rkYCon); + kQ[3] = -kQ[3]; + return kQ; +} +//---------------------------------------------------------------------------- +template +Quaternion Quaternion::GetClosestZX (const Constraints& rkZCon, + const Constraints& rkXCon) const +{ + Quaternion kQ, kTmp; + Real fC2, fS2, fC0, fS0, fInvLength; + + Real fDet = m_afTuple[0]*m_afTuple[2] - m_afTuple[1]*m_afTuple[3]; + if (Math::FAbs(fDet) < (Real)0.5 - ms_fTolerance) + { + Real fDiscr = Math::Sqrt(Math::FAbs((Real)1 - + ((Real)4)*fDet*fDet)); + Real fA = m_afTuple[0]*m_afTuple[3] + m_afTuple[1]*m_afTuple[2]; + Real fB = m_afTuple[0]*m_afTuple[0] + m_afTuple[1]*m_afTuple[1] + - m_afTuple[2]*m_afTuple[2] - m_afTuple[3]*m_afTuple[3]; + + if (fB >= (Real)0) + { + fC2 = ((Real)0.5)*(fDiscr + fB); + fS2 = fA; + } + else + { + fC2 = fA; + fS2 = ((Real)0.5)*(fDiscr - fB); + } + fInvLength = Math::InvSqrt(fC2*fC2 + fS2*fS2); + fC2 *= fInvLength; + fS2 *= fInvLength; + + fC0 = m_afTuple[0]*fC2 + m_afTuple[3]*fS2; + fS0 = m_afTuple[1]*fC2 + m_afTuple[2]*fS2; + fInvLength = Math::InvSqrt(fC0*fC0 + fS0*fS0); + fC0 *= fInvLength; + fS0 *= fInvLength; + + if (rkZCon.IsValid(fC2,fS2) && rkXCon.IsValid(fC0,fS0)) + { + // The maximum occurs at an interior point. + kQ[0] = fC2*fC0; + kQ[1] = fC2*fS0; + kQ[2] = fS2*fS0; + kQ[3] = fS2*fC0; + } + else + { + // The maximum occurs at a boundary point. + Quaternion kR(rkZCon.m_fCosMinAngle,(Real)0,(Real)0, + rkZCon.m_fSinMinAngle); + Quaternion kRInv(rkZCon.m_fCosMinAngle,(Real)0,(Real)0, + -rkZCon.m_fSinMinAngle); + Quaternion kProd = kRInv*(*this); + kTmp = kProd.GetClosest(1,rkXCon); + Real fDotOptAngle = kProd.Dot(kTmp); + kQ = kR*kTmp; + + kR = Quaternion(rkZCon.m_fCosMaxAngle,(Real)0,(Real)0, + rkZCon.m_fSinMaxAngle); + kRInv = Quaternion(rkZCon.m_fCosMaxAngle,(Real)0,(Real)0, + -rkZCon.m_fSinMaxAngle); + kProd = kRInv*(*this); + kTmp = kProd.GetClosest(1,rkXCon); + Real fDotAngle = kProd.Dot(kTmp); + if (fDotAngle > fDotOptAngle) + { + kQ = kR*kTmp; + fDotOptAngle = fDotAngle; + } + + kR = Quaternion(rkXCon.m_fCosMinAngle,rkXCon.m_fSinMinAngle, + (Real)0,(Real)0); + kRInv = Quaternion(rkXCon.m_fCosMinAngle,-rkXCon.m_fSinMinAngle, + (Real)0,(Real)0); + kProd = (*this)*kRInv; + kTmp = kProd.GetClosest(3,rkZCon); + fDotAngle = kProd.Dot(kTmp); + if (fDotAngle > fDotOptAngle) + { + kQ = kTmp*kR; + fDotOptAngle = fDotAngle; + } + + kR = Quaternion(rkXCon.m_fCosMaxAngle,rkXCon.m_fSinMaxAngle, + (Real)0,(Real)0); + kRInv = Quaternion(rkXCon.m_fCosMaxAngle,-rkXCon.m_fSinMaxAngle, + (Real)0,(Real)0); + kProd = (*this)*kRInv; + kTmp = kProd.GetClosest(3,rkZCon); + fDotAngle = kProd.Dot(kTmp); + if (fDotAngle > fDotOptAngle) + { + kQ = kTmp*kR; + fDotOptAngle = fDotAngle; + } + } + } + else + { + // Infinitely many solutions, choose one that satisfies the angle + // constraints. + Real fMinAngle, fMaxAngle, fAngle; + Constraints kCon; + + if (fDet > (Real)0) + { + fMinAngle = rkXCon.m_fMinAngle - rkZCon.m_fMaxAngle; + fMaxAngle = rkXCon.m_fMaxAngle - rkZCon.m_fMinAngle; + kCon.SetAngles(fMinAngle,fMaxAngle); + + kTmp = GetClosest(1,kCon); + + fAngle = Math::ATan2(kTmp[1],kTmp[0]); + if (fAngle < fMinAngle || fAngle > fMaxAngle) + { + fAngle -= + (kTmp[1] >= (Real)0 ? Math::PI : -Math::PI); + // assert(fMinAngle <= fAngle && fAngle <= fMaxAngle); + } + + if (fAngle <= rkXCon.m_fMaxAngle - rkZCon.m_fMaxAngle) + { + fC2 = rkZCon.m_fCosMaxAngle; + fS2 = rkZCon.m_fSinMaxAngle; + fAngle = rkZCon.m_fMaxAngle + fAngle; + fC0 = Math::Cos(fAngle); + fS0 = Math::Sin(fAngle); + } + else + { + fC0 = rkXCon.m_fCosMaxAngle; + fS0 = rkXCon.m_fSinMaxAngle; + fAngle = rkXCon.m_fMaxAngle - fAngle; + fC2 = Math::Cos(fAngle); + fS2 = Math::Sin(fAngle); + } + } + else + { + fMinAngle = rkXCon.m_fMinAngle + rkZCon.m_fMinAngle; + fMaxAngle = rkXCon.m_fMaxAngle + rkZCon.m_fMaxAngle; + kCon.SetAngles(fMinAngle,fMaxAngle); + + kTmp = GetClosest(1,kCon); + + fAngle = Math::ATan2(kTmp[1],kTmp[0]); + if (fAngle < fMinAngle || fAngle > fMaxAngle) + { + fAngle -= + (kTmp[1] >= (Real)0 ? Math::PI : -Math::PI); + // assert(fMinAngle <= fAngle && fAngle <= fMaxAngle); + } + + if (fAngle >= rkXCon.m_fMinAngle + rkZCon.m_fMaxAngle) + { + fC2 = rkZCon.m_fCosMaxAngle; + fS2 = rkZCon.m_fSinMaxAngle; + fAngle = fAngle - rkZCon.m_fMaxAngle; + fC0 = Math::Cos(fAngle); + fS0 = Math::Sin(fAngle); + } + else + { + fC0 = rkXCon.m_fCosMaxAngle; + fS0 = rkXCon.m_fSinMaxAngle; + fAngle = fAngle - rkXCon.m_fMaxAngle; + fC2 = Math::Cos(fAngle); + fS2 = Math::Sin(fAngle); + } + } + + kQ[0] = fC2*fC0; + kQ[1] = fC2*fS0; + kQ[2] = fS2*fS0; + kQ[3] = fS2*fC0; + if (Dot(kQ) < (Real)0) + { + kQ = -kQ; + } + } + + return kQ; +} +//---------------------------------------------------------------------------- +template +Quaternion Quaternion::GetClosestXZ (const Constraints& rkXCon, + const Constraints& rkZCon) const +{ + Quaternion pkAlt(m_afTuple[0],m_afTuple[1],-m_afTuple[2],m_afTuple[3]); + Quaternion kQ = pkAlt.GetClosestZX(rkZCon,rkXCon); + kQ[2] = -kQ[2]; + return kQ; +} +//---------------------------------------------------------------------------- +template +Quaternion Quaternion::GetClosestZY (const Constraints& rkZCon, + const Constraints& rkYCon) const +{ + Quaternion kQ, kTmp; + Real fC2, fS2, fC1, fS1, fInvLength; + + Real fDet = m_afTuple[0]*m_afTuple[1] + m_afTuple[2]*m_afTuple[3]; + if (Math::FAbs(fDet) < (Real)0.5 - ms_fTolerance) + { + Real fDiscr = Math::Sqrt(Math::FAbs((Real)1 - + ((Real)4)*fDet*fDet)); + Real fA = m_afTuple[0]*m_afTuple[3] - m_afTuple[1]*m_afTuple[2]; + Real fB = m_afTuple[0]*m_afTuple[0] - m_afTuple[1]*m_afTuple[1] + + m_afTuple[2]*m_afTuple[2] - m_afTuple[3]*m_afTuple[3]; + + if (fB >= (Real)0) + { + fC2 = ((Real)0.5)*(fDiscr + fB); + fS2 = fA; + } + else + { + fC2 = fA; + fS2 = ((Real)0.5)*(fDiscr - fB); + } + fInvLength = Math::InvSqrt(fC2*fC2 + fS2*fS2); + fC2 *= fInvLength; + fS2 *= fInvLength; + + fC1 = m_afTuple[0]*fC2 + m_afTuple[3]*fS2; + fS1 = m_afTuple[2]*fC2 - m_afTuple[1]*fS2; + fInvLength = Math::InvSqrt(fC1*fC1 + fS1*fS1); + fC1 *= fInvLength; + fS1 *= fInvLength; + + if (rkZCon.IsValid(fC2,fS2) && rkYCon.IsValid(fC1,fS1)) + { + // The maximum occurs at an interior point. + kQ[0] = fC2*fC1; + kQ[1] = -fS2*fS1; + kQ[2] = fC2*fS1; + kQ[3] = fS2*fC1; + } + else + { + // The maximum occurs at a boundary point. + Quaternion kR(rkZCon.m_fCosMinAngle,(Real)0,(Real)0, + rkZCon.m_fSinMinAngle); + Quaternion kRInv(rkZCon.m_fCosMinAngle,(Real)0,(Real)0, + -rkZCon.m_fSinMinAngle); + Quaternion kProd = kRInv*(*this); + kTmp = kProd.GetClosest(2,rkYCon); + Real fDotOptAngle = kProd.Dot(kTmp); + kQ = kR*kTmp; + + kR = Quaternion(rkZCon.m_fCosMaxAngle,(Real)0,(Real)0, + rkZCon.m_fSinMaxAngle); + kRInv = Quaternion(rkZCon.m_fCosMaxAngle,(Real)0,(Real)0, + -rkZCon.m_fSinMaxAngle); + kProd = kRInv*(*this); + kTmp = kProd.GetClosest(2,rkYCon); + Real fDotAngle = kProd.Dot(kTmp); + if (fDotAngle > fDotOptAngle) + { + kQ = kR*kTmp; + fDotOptAngle = fDotAngle; + } + + kR = Quaternion(rkYCon.m_fCosMinAngle,(Real)0, + rkYCon.m_fSinMinAngle,(Real)0); + kRInv = Quaternion(rkYCon.m_fCosMinAngle,(Real)0, + -rkYCon.m_fSinMinAngle,(Real)0); + kProd = (*this)*kRInv; + kTmp = kProd.GetClosest(3,rkZCon); + fDotAngle = kProd.Dot(kTmp); + if (fDotAngle > fDotOptAngle) + { + kQ = kTmp*kR; + fDotOptAngle = fDotAngle; + } + + kR = Quaternion(rkYCon.m_fCosMaxAngle,(Real)0, + rkYCon.m_fSinMaxAngle,(Real)0); + kRInv = Quaternion(rkYCon.m_fCosMaxAngle,(Real)0, + -rkYCon.m_fSinMaxAngle,(Real)0); + kProd = (*this)*kRInv; + kTmp = kProd.GetClosest(3,rkZCon); + fDotAngle = kProd.Dot(kTmp); + if (fDotAngle > fDotOptAngle) + { + kQ = kTmp*kR; + fDotOptAngle = fDotAngle; + } + } + } + else + { + // Infinitely many solutions, choose one that satisfies the angle + // constraints. + Real fMinAngle, fMaxAngle, fAngle; + Constraints kCon; + + if (fDet < (Real)0) + { + fMinAngle = rkYCon.m_fMinAngle - rkZCon.m_fMaxAngle; + fMaxAngle = rkYCon.m_fMaxAngle - rkZCon.m_fMinAngle; + kCon.SetAngles(fMinAngle,fMaxAngle); + + kTmp = GetClosest(2,kCon); + + fAngle = Math::ATan2(kTmp[2],kTmp[0]); + if (fAngle < fMinAngle || fAngle > fMaxAngle) + { + fAngle -= + (kTmp[2] >= (Real)0 ? Math::PI : -Math::PI); + // assert(fMinAngle <= fAngle && fAngle <= fMaxAngle); + } + + if (fAngle <= rkYCon.m_fMaxAngle - rkZCon.m_fMaxAngle) + { + fC2 = rkZCon.m_fCosMaxAngle; + fS2 = rkZCon.m_fSinMaxAngle; + fAngle = rkZCon.m_fMaxAngle + fAngle; + fC1 = Math::Cos(fAngle); + fS1 = Math::Sin(fAngle); + } + else + { + fC1 = rkYCon.m_fCosMaxAngle; + fS1 = rkYCon.m_fSinMaxAngle; + fAngle = rkYCon.m_fMaxAngle - fAngle; + fC2 = Math::Cos(fAngle); + fS2 = Math::Sin(fAngle); + } + } + else + { + fMinAngle = rkYCon.m_fMinAngle + rkZCon.m_fMinAngle; + fMaxAngle = rkYCon.m_fMaxAngle + rkZCon.m_fMaxAngle; + kCon.SetAngles(fMinAngle,fMaxAngle); + + kTmp = GetClosest(2,kCon); + + fAngle = Math::ATan2(kTmp[2],kTmp[0]); + if (fAngle < fMinAngle || fAngle > fMaxAngle) + { + fAngle -= + (kTmp[2] >= (Real)0 ? Math::PI : -Math::PI); + // assert(fMinAngle <= fAngle && fAngle <= fMaxAngle); + } + + if (fAngle >= rkYCon.m_fMinAngle + rkZCon.m_fMaxAngle) + { + fC2 = rkZCon.m_fCosMaxAngle; + fS2 = rkZCon.m_fSinMaxAngle; + fAngle = fAngle - rkZCon.m_fMaxAngle; + fC1 = Math::Cos(fAngle); + fS1 = Math::Sin(fAngle); + } + else + { + fC1 = rkYCon.m_fCosMaxAngle; + fS1 = rkYCon.m_fSinMaxAngle; + fAngle = fAngle - rkYCon.m_fMaxAngle; + fC2 = Math::Cos(fAngle); + fS2 = Math::Sin(fAngle); + } + } + + kQ[0] = fC2*fC1; + kQ[1] = -fS2*fS1; + kQ[2] = fC2*fS1; + kQ[3] = fS2*fC1; + if (Dot(kQ) < (Real)0) + { + kQ = -kQ; + } + } + + return kQ; +} +//---------------------------------------------------------------------------- +template +Quaternion Quaternion::GetClosestYZ (const Constraints& rkYCon, + const Constraints& rkZCon) const +{ + Quaternion pkAlt(m_afTuple[0],-m_afTuple[1],m_afTuple[2],m_afTuple[3]); + Quaternion kQ = pkAlt.GetClosestZY(rkZCon,rkYCon); + kQ[1] = -kQ[1]; + return kQ; +} +//----------------------------------------------------------------------------