+ return SubContainsPoint(rkP, iMid, i1);
+ }
+}
+//----------------------------------------------------------------------------
+bool ConvexHull2D::ContainsPoint(const Vector2 &rkP) const
+{
+#if 0
+ // O(N) algorithm
+ for (int i1 = 0, i0 = m_iHQuantity-1; i1 < m_iHQuantity; i0 = i1++)
+ {
+ const Vector2& rkV0 = m_akVertex[m_aiHIndex[i0]];
+ const Vector2& rkV1 = m_akVertex[m_aiHIndex[i1]];
+ Vector2 kDir = rkV1 - rkV0;
+ Vector2 kNormal = kDir.Cross(); // outer normal
+ if ( kNormal.Dot(rkP-rkV0) > 0.0f )
+ return false;
+ }
+
+ return true;
+#else
+ // O(log N) algorithm
+ if (m_iHQuantity > 2)
+ {
+ return SubContainsPoint(rkP, 0, 0);
+ }
+ else if (m_iHQuantity == 2)
+ {
+ int iTest = CollinearTest(rkP, m_akVertex[m_aiHIndex[0]],
+ m_akVertex[m_aiHIndex[1]]);
+
+ return (iTest == ORDER_COLLINEAR_CONTAIN);
+ }
+ else // assert: m_iHQuantity == 1
+ {
+ return rkP == m_akVertex[0];
+ }
+#endif
+}
+//----------------------------------------------------------------------------
+
+//----------------------------------------------------------------------------
+// support for both hull methods
+//----------------------------------------------------------------------------
+bool ConvexHull2D::SortedVertex::operator==(const SortedVertex &rkSV) const
+{
+ return m_kV == rkSV.m_kV;
+}
+//----------------------------------------------------------------------------
+bool ConvexHull2D::SortedVertex::operator<(const SortedVertex &rkSV) const
+{
+ Real fXTmp = rkSV.m_kV.x;
+ if (Math::FAbs(m_kV.x - fXTmp) <= Vector2::FUZZ)
+ fXTmp = m_kV.x;
+
+ if (m_kV.x < fXTmp)
+ return true;
+ else if (m_kV.x > fXTmp)
+ return false;
+
+ Real fYTmp = rkSV.m_kV.y;
+ if (Math::FAbs(m_kV.y - fYTmp) <= Vector2::FUZZ)
+ fYTmp = m_kV.y;
+
+ return m_kV.y < fYTmp;
+}
+//----------------------------------------------------------------------------
+int ConvexHull2D::CollinearTest(const Vector2 &rkP, const Vector2 &rkQ0,
+ const Vector2 &rkQ1) const
+{
+ Vector2 kD = rkQ1 - rkQ0;
+ Vector2 kA = rkP - rkQ0;
+ Real fDdD = kD.Dot(kD);
+ Real fAdA = kA.Dot(kA);
+ Real fDet = kD.x * kA.y - kD.y * kA.x;
+ Real fRelative = fDet * fDet - ms_fColEpsilon * fDdD * fAdA;
+
+ if (fRelative > 0.0f)
+ {
+ if (fDet > 0.0f)
+ {
+ // points form counterclockwise triangle
+ return ORDER_POSITIVE;
+ }
+ else if (fDet < 0.0f)
+ {
+ // points form clockwise triangle
+ return ORDER_NEGATIVE;
+ }
+ }
+
+ // P is on line of
+ Real fDdA = kD.Dot(kA);
+ if (fDdA < 0.0f)
+ {
+ // order is
+ return ORDER_COLLINEAR_LEFT;
+ }
+
+ if (fDdA > fDdD)
+ {
+ // order is
+ return ORDER_COLLINEAR_RIGHT;
+ }
+
+ // order is
+ return ORDER_COLLINEAR_CONTAIN;
+}
+//----------------------------------------------------------------------------
+void ConvexHull2D::RemoveCollinear()
+{
+ if (m_iHQuantity <= 2)
+ return;
+
+ Real fSaveFuzz = Vector2::FUZZ;
+ Vector2::FUZZ = ms_fVeqEpsilon;
+
+ vector kHull;
+ kHull.reserve(m_iHQuantity);
+ for (int i0 = m_iHQuantity - 1, i1 = 0, i2 = 1; i1 < m_iHQuantity; /**/)
+ {
+ int iCT = CollinearTest(m_akVertex[m_aiHIndex[i0]],
+ m_akVertex[m_aiHIndex[i1]], m_akVertex[m_aiHIndex[i2]]);
+
+ if (iCT == ORDER_POSITIVE || iCT == ORDER_NEGATIVE)
+ {
+ // points are not collinear
+ kHull.push_back(m_aiHIndex[i1]);
+ }
+
+ i0 = i1++;
+ if (++i2 == m_iHQuantity)
+ i2 = 0;
+ }
+
+ // construct index array for ordered vertices of convex hull
+ m_iHQuantity = kHull.size();
+ delete[] m_aiHIndex;
+ m_aiHIndex = new int[m_iHQuantity];
+ memcpy(m_aiHIndex, &kHull.front(), m_iHQuantity * sizeof(int));
+
+ Vector2::FUZZ = fSaveFuzz;
+}
+//----------------------------------------------------------------------------
+
+//----------------------------------------------------------------------------
+// divide-and-conquer hull
+//----------------------------------------------------------------------------
+void ConvexHull2D::ByDivideAndConquer()
+{
+ Real fSaveFuzz = Vector2::FUZZ;
+ Vector2::FUZZ = ms_fVeqEpsilon;
+
+ // Sort by x-component and store in contiguous array. The sort is
+ // O(N log N).
+ SVArray kSVArray(m_iVQuantity);
+ int i;
+ for (i = 0; i < m_iVQuantity; i++)
+ {
+ kSVArray[i].m_kV = m_akVertex[i];
+ kSVArray[i].m_iIndex = i;
+ }
+ sort(kSVArray.begin(), kSVArray.end());
+
+ // remove duplicate points
+ SVArray::iterator pkEnd = unique(kSVArray.begin(), kSVArray.end());
+ kSVArray.erase(pkEnd, kSVArray.end());
+
+ // compute convex hull using divide-and-conquer
+ SVArray kHull;
+ kHull.reserve(kSVArray.size());
+ m_iHullType = HULL_POINT;
+ GetHull(0, kSVArray.size() - 1, kSVArray, kHull);
+
+ // construct index array for ordered vertices of convex hull
+ m_iHQuantity = kHull.size();
+ m_aiHIndex = new int[m_iHQuantity];
+ for (i = 0; i < m_iHQuantity; i++)
+ m_aiHIndex[i] = kHull[i].m_iIndex;
+
+ Vector2::FUZZ = fSaveFuzz;
+}
+//----------------------------------------------------------------------------
+void ConvexHull2D::GetHull(int i0, int i1, const SVArray &rkSVArray,
+ SVArray &rkHull)
+{
+ int iQuantity = i1 - i0 + 1;
+ if (iQuantity > 1)
+ {
+ // middle index of input range
+ int iMid = (i0 + i1) / 2;
+
+ // find hull of subsets (iMid-i0+1 >= i1-iMid)
+ SVArray kLHull, kRHull;
+ kLHull.reserve(iMid - i0 + 1);
+ kRHull.reserve(i1 - iMid);
+
+ GetHull(i0, iMid, rkSVArray, kLHull);
+ GetHull(iMid + 1, i1, rkSVArray, kRHull);
+
+ // merge the convex hulls into a single convex hull
+ Merge(kLHull, kRHull, rkHull);
+ }
+ else
+ {
+ // convex hull is a single point
+ rkHull.push_back(rkSVArray[i0]);
+ }
+}
+//----------------------------------------------------------------------------
+void ConvexHull2D::Merge(SVArray &rkLHull, SVArray &rkRHull, SVArray &rkHull)
+{
+ // merge small sets, handle cases when sets are collinear
+ int iLSize = rkLHull.size(), iRSize = rkRHull.size();
+ if (iLSize == 1)
+ {
+ if (iRSize == 1)
+ {
+ rkHull.push_back(rkLHull[0]);
+ rkHull.push_back(rkRHull[0]);
+ if (m_iHullType != HULL_PLANAR)
+ m_iHullType = HULL_LINEAR;
+ return;
+ }
+ else if (iRSize == 2)
+ {
+ MergeLinear(rkLHull[0], rkRHull);
+ rkHull = rkRHull;
+ return;
+ }
+ }
+ else if (iLSize == 2)
+ {
+ if (iRSize == 1)
+ {
+ MergeLinear(rkRHull[0], rkLHull);
+ rkHull = rkLHull;
+ return;
+ }
+ else if (iRSize == 2)
+ {
+ MergeLinear(rkRHull[1], rkLHull);
+
+ if (rkLHull.size() == 2)
+ {
+ MergeLinear(rkRHull[0], rkLHull);
+ rkHull = rkLHull;
+ return;
+ }
+
+ // fall-through, LHull is a triangle, RHull is a point
+ iLSize = 3;
+ iRSize = 1;
+ rkRHull.pop_back();
+ }
+ }
+
+ // find rightmost point of left hull
+ Real fMax = rkLHull[0].m_kV.x;
+ int i, iLMax = 0;
+ for (i = 1; i < iLSize; i++)
+ {
+ if (rkLHull[i].m_kV.x > fMax)
+ {
+ fMax = rkLHull[i].m_kV.x;
+ iLMax = i;
+ }
+ }
+
+ // find leftmost point of right hull
+ Real fMin = rkRHull[0].m_kV.x;
+ int iRMin = 0;
+ for (i = 1; i < iRSize; i++)
+ {
+ if (rkRHull[i].m_kV.x < fMin)
+ {
+ fMin = rkRHull[i].m_kV.x;
+ iRMin = i;
+ }
+ }
+
+ // get lower tangent to hulls (LL = lower left, LR = lower right)
+ int iLLIndex = iLMax, iLRIndex = iRMin;
+ GetTangent(rkLHull, rkRHull, iLLIndex, iLRIndex);
+
+ // get upper tangent to hulls (UL = upper left, UR = upper right)
+ int iULIndex = iLMax, iURIndex = iRMin;
+ GetTangent(rkRHull, rkLHull, iURIndex, iULIndex);
+
+ // Construct the counterclockwise-ordered merged-hull vertices.
+ // TO DO. If the hull is stored using linked lists, this block can be
+ // improved by an O(1) detach, attach, and delete of the appropriate
+ // sublists.
+ i = iLRIndex;
+ while (true)
+ {
+ rkHull.push_back(rkRHull[i]);
+ if (i == iURIndex)
+ break;
+
+ if (++i == iRSize)
+ i = 0;
+ }
+
+ i = iULIndex;
+ while (true)
+ {
+ rkHull.push_back(rkLHull[i]);
+ if (i == iLLIndex)
+ break;
+
+ if (++i == iLSize)
+ i = 0;
+ }
+}
+//----------------------------------------------------------------------------
+void ConvexHull2D::MergeLinear(const SortedVertex &rkP, SVArray &rkHull)
+{
+ // hull =
+
+ switch (CollinearTest(rkP.m_kV, rkHull[0].m_kV, rkHull[1].m_kV))
+ {
+ case ORDER_POSITIVE:
+ // merged hull is triangle
+ m_iHullType = HULL_PLANAR;
+ rkHull.push_back(rkP);
+ break;
+ case ORDER_NEGATIVE:
+ {
+ // merged hull is triangle
+ m_iHullType = HULL_PLANAR;
+ SortedVertex kSave = rkHull[0];
+ rkHull[0] = rkHull[1];
+ rkHull[1] = kSave;
+ rkHull.push_back(rkP);
+ break;
+ }
+ case ORDER_COLLINEAR_LEFT:
+ // collinear order is , merged hull is
+ rkHull[0] = rkP;
+ break;
+ case ORDER_COLLINEAR_RIGHT:
+ // collinear order is , merged hull is
+ rkHull[1] = rkP;
+ break;
+ // case ORDER_COLLINEAR_CONTAIN:
+ // collinear order is , merged hull is (no change)
+ }
+}
+//----------------------------------------------------------------------------
+void ConvexHull2D::GetTangent(const SVArray &rkLHull, const SVArray &rkRHull,
+ int &riL, int &riR)
+{
+ // In theory the loop terminates in a finite number of steps, but the
+ // upper bound for the loop variable is used to trap problems caused by
+ // floating point round-off errors that might lead to an infinite loop.
+
+ int iLSize = rkLHull.size(), iRSize = rkRHull.size();
+ int iLSizeM1 = iLSize - 1, iRSizeM1 = iRSize - 1;
+ int i;
+ for (i = 0; i < iLSize + iRSize; i++)
+ {
+ // end points of potential tangent
+ const Vector2 &rkL1 = rkLHull[riL].m_kV;
+ const Vector2 &rkR0 = rkRHull[riR].m_kV;
+
+ // walk along left hull to find tangency
+ int iLm1 = (riL > 0 ? riL - 1 : iLSizeM1);
+ const Vector2 &rkL0 = rkLHull[iLm1].m_kV;
+ int iCT = CollinearTest(rkR0, rkL0, rkL1);
+ if (iCT == ORDER_NEGATIVE || iCT == ORDER_COLLINEAR_LEFT)
+ {
+ riL = iLm1;
+ continue;
+ }
+
+ // walk along right hull to find tangency
+ int iRp1 = (riR < iRSizeM1 ? riR + 1 : 0);
+ const Vector2 &rkR1 = rkRHull[iRp1].m_kV;
+ iCT = CollinearTest(rkL1, rkR0, rkR1);
+ if (iCT == ORDER_NEGATIVE || iCT == ORDER_COLLINEAR_RIGHT)
+ {
+ riR = iRp1;
+ continue;
+ }
+
+ // tangent segment has been found
+ break;
+ }
+
+ // detect "infinite loop" caused by floating point round-off errors
+ // VERIFY( i < iLSize+iRSize );
+}
+//----------------------------------------------------------------------------
+
+//----------------------------------------------------------------------------
+// incremental hull
+//----------------------------------------------------------------------------
+void ConvexHull2D::ByIncremental()
+{
+ Real fSaveFuzz = Vector2::FUZZ;
+ Vector2::FUZZ = ms_fVeqEpsilon;
+
+ // Sort by x-component and store in contiguous array. The sort is
+ // O(N log N).
+ SVArray kSVArray(m_iVQuantity);
+ int i;
+ for (i = 0; i < m_iVQuantity; i++)
+ {
+ kSVArray[i].m_kV = m_akVertex[i];
+ kSVArray[i].m_iIndex = i;
+ }
+ sort(kSVArray.begin(), kSVArray.end());
+
+ // remove duplicate points
+ SVArray::iterator pkEnd = unique(kSVArray.begin(), kSVArray.end());
+ kSVArray.erase(pkEnd, kSVArray.end());
+
+ int ccc = kSVArray.size();
+
+ // Compute convex hull incrementally. The first and second vertices in
+ // the hull are managed separately until at least one triangle is formed.
+ // At that time an array is used to store the hull in counterclockwise
+ // order.
+ m_iHullType = HULL_POINT;
+ m_kHull.push_back(kSVArray[0]);
+ for (i = 1; i < (int)kSVArray.size(); i++)
+ {
+ switch (m_iHullType)
+ {
+ case HULL_POINT:
+ m_iHullType = HULL_LINEAR;
+ m_kHull.push_back(kSVArray[i]);
+ break;
+ case HULL_LINEAR:
+ MergeLinear(kSVArray[i]);
+ break;
+ case HULL_PLANAR:
+ MergePlanar(kSVArray[i]);
+ break;
+ }
+ }
+
+ // construct index array for ordered vertices of convex hull
+ m_iHQuantity = m_kHull.size();
+ m_aiHIndex = new int[m_iHQuantity];
+ for (i = 0; i < m_iHQuantity; i++)
+ m_aiHIndex[i] = m_kHull[i].m_iIndex;
+
+ Vector2::FUZZ = fSaveFuzz;
+}
+//----------------------------------------------------------------------------
+void ConvexHull2D::MergeLinear(const SortedVertex &rkP)
+{
+ switch (CollinearTest(rkP.m_kV, m_kHull[0].m_kV, m_kHull[1].m_kV))
+ {
+ case ORDER_POSITIVE:
+ // merged hull is
+ m_iHullType = HULL_PLANAR;
+ m_kHull.push_back(m_kHull[1]);
+ m_kHull[1] = m_kHull[0];
+ m_kHull[0] = rkP;
+ break;
+ case ORDER_NEGATIVE:
+ // merged hull is
+ m_iHullType = HULL_PLANAR;
+ m_kHull.push_back(m_kHull[0]);
+ m_kHull[0] = rkP;
+ break;
+ case ORDER_COLLINEAR_LEFT:
+ // linear order is
, merged hull is
+ m_kHull[0] = rkP;
+ break;
+ case ORDER_COLLINEAR_RIGHT:
+ // linear order is , merged hull is
+ m_kHull[1] = rkP;
+ break;
+ // case ORDER_COLLINEAR_CONTAIN: linear order is , no change
+ }
+}
+//----------------------------------------------------------------------------
+void ConvexHull2D::MergePlanar(const SortedVertex &rkP)
+{
+ int iSize = m_kHull.size();
+ int i, iU, iL, iCT;
+
+ // search counterclockwise for last visible vertex
+ for (iU = 0, i = 1; iU < iSize; iU = i++)
+ {
+ if (i == iSize)
+ i = 0;
+
+ iCT = CollinearTest(rkP.m_kV, m_kHull[iU].m_kV, m_kHull[i].m_kV);
+ if (iCT == ORDER_NEGATIVE)
+ continue;
+ if (iCT == ORDER_POSITIVE || iCT == ORDER_COLLINEAR_LEFT)
+ break;
+
+ // iCT == ORDER_COLLINEAR_CONTAIN || iCT == ORDER_COLLINEAR_RIGHT
+ return;
+ }
+ assert(iU < iSize);
+
+ // search clockwise for last visible vertex
+ for (iL = 0, i = iSize - 1; i >= 0; iL = i--)
+ {
+ iCT = CollinearTest(rkP.m_kV, m_kHull[i].m_kV, m_kHull[iL].m_kV);
+ if (iCT == ORDER_NEGATIVE)
+ continue;
+ if (iCT == ORDER_POSITIVE || iCT == ORDER_COLLINEAR_RIGHT)
+ break;
+
+ // iCT == ORDER_COLLINEAR_CONTAIN || iCT == ORDER_COLLINEAR_LEFT
+ return;
+ }
+ assert(i >= 0);
+
+ // construct the counterclockwise-ordered merged-hull vertices
+ SVArray kTmpHull;
+ kTmpHull.push_back(rkP);
+ while (true)
+ {
+ kTmpHull.push_back(m_kHull[iU]);
+ if (iU == iL)
+ break;
+
+ if (++iU == iSize)
+ iU = 0;
+ }
+ if (kTmpHull.size() > 2)
+ m_kHull = kTmpHull;
+ else
+ printf("error in ConvexHull2D::MergePlanar");
+}
+//----------------------------------------------------------------------------
diff --git a/src/editors/FreeMagic/MgcConvexHull2D.h b/src/editors/FreeMagic/MgcConvexHull2D.h
new file mode 100644
index 00000000000..9c02219fbc0
--- /dev/null
+++ b/src/editors/FreeMagic/MgcConvexHull2D.h
@@ -0,0 +1,135 @@
+// Magic Software, Inc.
+// http://www.magic-software.com
+// Copyright (c) 2000-2002. All Rights Reserved
+//
+// Source code from Magic Software is supplied under the terms of a license
+// agreement and may not be copied or disclosed except in accordance with the
+// terms of that agreement. The various license agreements may be found at
+// the Magic Software web site. This file is subject to the license
+//
+// FREE SOURCE CODE
+// http://www.magic-software.com/License/free.pdf
+
+#ifndef MGCCONVEXHULL2D_H
+#define MGCCONVEXHULL2D_H
+
+#include "MgcVector2.h"
+#include
+
+namespace Mgc
+{
+
+ class MAGICFM ConvexHull2D
+ {
+ public:
+ // Construction and destruction. ConvexHull2D does not take ownership
+ // of the input array. The application is responsible for deleting it.
+ ConvexHull2D(int iVQuantity, const Vector2 *akVertex);
+ ~ConvexHull2D();
+
+ // two different methods to compute convex hull
+ void ByDivideAndConquer();
+ void ByIncremental();
+
+ // hull stored in counterclockwise order
+ int GetQuantity() const;
+ const int *GetIndices() const;
+
+ // remove collinear points on hull
+ void RemoveCollinear();
+
+ // test if point is contained by hull
+ bool ContainsPoint(const Vector2 &rkP) const;
+
+ // The 'vertex equality' epsilon is used to set Vector2::FUZZ for fuzzy
+ // equality of vector components. Two vectors (x0,y0) and (x1,y1) are
+ // considered equal if |x1-x0| <= FUZZ and |y1-y0| <= FUZZ. Observe
+ // that FUZZ = 0 yields an exact equality test. The default value is 0.0.
+ static Real &VertexEqualityEpsilon();
+
+ // The 'collinear epsilon' is used to test if three points P0, P1, and P2
+ // are collinear. If A = P1-P0 and B = P2-P0, the points are collinear
+ // in theory if d = A.x*B.y-A.y*B.x = 0. For numerical robustness, the
+ // test is implemented as |d|^2 <= e*|A|^2*|B|^2 where e is the collinear
+ // epsilon. The idea is that d = |Cross((A,0),(B,0))| = |A|*|B|*|sin(t)|
+ // where t is the angle between A and B. Therefore, the comparison is
+ // really |sin(t)|^2 <= e, a relative error test. The default e = 1e-06.
+ static Real &CollinearEpsilon();
+
+ protected:
+ // support for O(log N) point-in-hull test
+ bool SubContainsPoint(const Vector2 &rkP, int i0, int i1) const;
+
+ // for sorting
+ class SortedVertex
+ {
+ public:
+ bool operator==(const SortedVertex &rkSV) const;
+ bool operator<(const SortedVertex &rkSV) const;
+
+ // Added to satisfy the SGI Mips Pro CC compiler that appears to be
+ // instantiating this, but never using it.
+ bool operator!=(const SortedVertex &rkSV) const
+ {
+ return !operator==(rkSV);
+ }
+
+ Vector2 m_kV;
+ int m_iIndex;
+ };
+
+ typedef std::vector SVArray;
+
+ // hull dimensions
+ enum
+ {
+ HULL_POINT,
+ HULL_LINEAR,
+ HULL_PLANAR
+ };
+
+ // for collinearity tests
+ enum
+ {
+ ORDER_POSITIVE,
+ ORDER_NEGATIVE,
+ ORDER_COLLINEAR_LEFT,
+ ORDER_COLLINEAR_RIGHT,
+ ORDER_COLLINEAR_CONTAIN
+ };
+
+ int CollinearTest(const Vector2 &rkP, const Vector2 &rkQ0,
+ const Vector2 &rkQ1) const;
+
+ // construct convex hull using divide-and-conquer
+ void GetHull(int i0, int i1, const SVArray &rkSVArray, SVArray &rkHull);
+ void Merge(SVArray &rkLHull, SVArray &rkRHull, SVArray &rkHull);
+ void MergeLinear(const SortedVertex &rkP, SVArray &rkHull);
+ void GetTangent(const SVArray &rkLHull, const SVArray &rkRHull,
+ int &riL, int &riR);
+
+ // construct convex hull incrementally
+ void MergeLinear(const SortedVertex &rkP);
+ void MergePlanar(const SortedVertex &rkP);
+
+ // vertex information
+ int m_iVQuantity;
+ const Vector2 *m_akVertex;
+
+ // hull information
+ int m_iHullType;
+ SVArray m_kHull;
+
+ // indices for ordered vertices of hull
+ int m_iHQuantity;
+ int *m_aiHIndex;
+
+ static Real ms_fVeqEpsilon;
+ static Real ms_fColEpsilon;
+ };
+
+#include "MgcConvexHull2D.inl"
+
+} // namespace Mgc
+
+#endif
diff --git a/src/editors/FreeMagic/MgcConvexHull2D.inl b/src/editors/FreeMagic/MgcConvexHull2D.inl
new file mode 100644
index 00000000000..b88b9bbb930
--- /dev/null
+++ b/src/editors/FreeMagic/MgcConvexHull2D.inl
@@ -0,0 +1,33 @@
+// Magic Software, Inc.
+// http://www.magic-software.com
+// Copyright (c) 2000-2002. All Rights Reserved
+//
+// Source code from Magic Software is supplied under the terms of a license
+// agreement and may not be copied or disclosed except in accordance with the
+// terms of that agreement. The various license agreements may be found at
+// the Magic Software web site. This file is subject to the license
+//
+// FREE SOURCE CODE
+// http://www.magic-software.com/License/free.pdf
+
+//----------------------------------------------------------------------------
+inline int ConvexHull2D::GetQuantity() const
+{
+ return m_iHQuantity;
+}
+//----------------------------------------------------------------------------
+inline const int *ConvexHull2D::GetIndices() const
+{
+ return m_aiHIndex;
+}
+//----------------------------------------------------------------------------
+inline Real &ConvexHull2D::VertexEqualityEpsilon()
+{
+ return ms_fVeqEpsilon;
+}
+//----------------------------------------------------------------------------
+inline Real &ConvexHull2D::CollinearEpsilon()
+{
+ return ms_fColEpsilon;
+}
+//----------------------------------------------------------------------------
diff --git a/src/editors/FreeMagic/MgcConvexHull3D.h b/src/editors/FreeMagic/MgcConvexHull3D.h
new file mode 100644
index 00000000000..27f12b9b5fa
--- /dev/null
+++ b/src/editors/FreeMagic/MgcConvexHull3D.h
@@ -0,0 +1,174 @@
+// Magic Software, Inc.
+// http://www.magic-software.com
+// Copyright (c) 2000-2002. All Rights Reserved
+//
+// Source code from Magic Software is supplied under the terms of a license
+// agreement and may not be copied or disclosed except in accordance with the
+// terms of that agreement. The various license agreements may be found at
+// the Magic Software web site. This file is subject to the license
+//
+// FREE SOURCE CODE
+// http://www.magic-software.com/License/free.pdf
+
+#ifndef MGCCONVEXHULL3D_H
+#define MGCCONVEXHULL3D_H
+
+#include "MgcTriangleMesh.h"
+#include "MgcVector3.h"
+#include
+
+namespace Mgc
+{
+
+ class MAGICFM ConvexHull3D
+ {
+ public:
+ // Construction and destruction. ConvexHull3D does not take ownership
+ // of the input array. The application is responsible for deleting it.
+ ConvexHull3D(int iVQuantity, const Vector3 *akVertex);
+ ~ConvexHull3D();
+
+ // Construct the hull a point at a time. This is an O(N^2) algorithm.
+ void ByIncremental();
+
+ // TO DO. Implement 'void ByDivideAndConquer'. This is an O(N log N)
+ // algorithm.
+
+ // Hull types. The quantity and indices are interpreted as follows.
+ enum
+ {
+ // Hull is a single point. Quantity is 1, index array has one
+ // element (index 0).
+ HULL_POINT,
+
+ // Hull is a line segment. Quantity is 2, index array has two
+ // elements that are indices to the end points of the segment.
+ HULL_LINEAR,
+
+ // Hull is a planar convex polygon. Quantity is number of vertices,
+ // index array has the indices to that number of vertices. The
+ // indices represent an ordered polygon, but since there is no
+ // associated normal vector, you need to supply your own and determine
+ // if the ordering is clockwise or counterclockwise relative to that
+ // normal. If you want a triangle connectivity array, you will have
+ // to triangulate the polygon yourself.
+ HULL_PLANAR,
+
+ // The hull is a convex polyhedron (positive volume). Quantity is
+ // number of triangles, index array has 3 times that many elements.
+ // Each triple of indices represents a triangle in the hull. All the
+ // triangles are counterclockwise ordered as you view the polyhedron
+ // from the outside.
+ HULL_SPATIAL
+ };
+
+ int GetType() const;
+ int GetQuantity() const;
+ const int *GetIndices() const;
+
+ // Remove collinear points in edges of planar hull. Has no effect on
+ // hulls with positive volume.
+ void RemoveCollinear();
+
+ // TO DO. Implement 'void RemoveCoplanar ()'. The result is a convex
+ // polyhedron whose faces are non-coplanar convex polygons, not
+ // necessarily triangles. After such a call, GetQuantity() should return
+ // the number of faces. GetIndices() will return an integer array that
+ // consists of subarrays, each subarray corresponding to a face. The
+ // first index of the subarray is number of indices for that face, those
+ // indices stored in the remainder of the subblock.
+
+ // The 'vertex equality' epsilon is used to set Vector3::FUZZ for fuzzy
+ // equality of vector components. Two vectors (x0,y0,z0) and (x1,y1,z1)
+ // are considered equal if |x1-x0| <= FUZZ, |y1-y0| <= FUZZ, and
+ // |z1-z0| <= FUZZ. Observe that FUZZ = 0 yields an exact equality test.
+ // The default value is 0.0.
+ static Real &VertexEqualityEpsilon();
+
+ // The 'collinear epsilon' is used to test if three points P0, P1, and P2
+ // are collinear. This is a relative error test on the sine of the angle
+ // between P1-P0 and P2-P0. The default value is 1e-06.
+ static Real &CollinearEpsilon();
+
+ // The 'coplanar epsilon' is used to test if four points P0, P1, P2, and
+ // P3 are coplanar. This is a relative error test on the volume of the
+ // tetrahedron formed by the four points. The default value is 1e-06.
+ static Real &CoplanarEpsilon();
+
+ protected:
+ // for sorting
+ class SortedVertex
+ {
+ public:
+ bool operator==(const SortedVertex &rkSV) const;
+ bool operator<(const SortedVertex &rkSV) const;
+
+ // Added to satisfy the SGI Mips Pro CC compiler that appears to be
+ // instantiating this, but never using it.
+ bool operator!=(const SortedVertex &rkSV) const
+ {
+ return !operator==(rkSV);
+ }
+
+ Vector3 m_kV;
+ int m_iIndex;
+ };
+
+ typedef std::vector SVArray;
+
+ // for collinearity and coplanaritytests
+ enum
+ {
+ ORDER_TRIANGLE,
+ ORDER_COLLINEAR_LEFT,
+ ORDER_COLLINEAR_RIGHT,
+ ORDER_COLLINEAR_CONTAIN,
+ ORDER_POSITIVE,
+ ORDER_NEGATIVE,
+ ORDER_COPLANAR,
+ ORDER_COPLANAR_INSIDE,
+ ORDER_COPLANAR_OUTSIDE
+ };
+
+ int CollinearTest(const Vector3 &rkP, const Vector3 &rkQ0,
+ const Vector3 &rkQ1) const;
+
+ int CoplanarTest(const Vector3 &rkP, const Vector3 &rkQ0,
+ const Vector3 &rkQ1, const Vector3 &rkQ2) const;
+
+ // test uses the plane defined by m_kPlaneOrigin and m_kPlaneNormal
+ int CoplanarTest(const Vector3 &rkP) const;
+
+ // construct convex hull incrementally
+ void MergeLinear(const SortedVertex &rkP);
+ void MergePlanar(const SortedVertex &rkP);
+ void MergeSpatial(const SortedVertex &rkP);
+
+ // vertex information
+ int m_iVQuantity;
+ const Vector3 *m_akVertex;
+
+ // hull information
+ int m_iHQuantity;
+ int *m_aiHIndex;
+ int m_iHullType;
+
+ // linear or planar hull
+ SVArray m_kHullP;
+ Vector3 m_kPlaneOrigin, m_kPlaneNormal;
+
+ // spatial hull
+ TriangleMesh m_kHullS;
+ int m_iLastIndex;
+
+ // tweaking parameters
+ static Real ms_fVertexEqualityEpsilon;
+ static Real ms_fCollinearEpsilon;
+ static Real ms_fCoplanarEpsilon;
+ };
+
+#include "MgcConvexHull3D.inl"
+
+} // namespace Mgc
+
+#endif
diff --git a/src/editors/FreeMagic/MgcConvexHull3D.inl b/src/editors/FreeMagic/MgcConvexHull3D.inl
new file mode 100644
index 00000000000..fda8d0539e2
--- /dev/null
+++ b/src/editors/FreeMagic/MgcConvexHull3D.inl
@@ -0,0 +1,43 @@
+// Magic Software, Inc.
+// http://www.magic-software.com
+// Copyright (c) 2000-2002. All Rights Reserved
+//
+// Source code from Magic Software is supplied under the terms of a license
+// agreement and may not be copied or disclosed except in accordance with the
+// terms of that agreement. The various license agreements may be found at
+// the Magic Software web site. This file is subject to the license
+//
+// FREE SOURCE CODE
+// http://www.magic-software.com/License/free.pdf
+
+//----------------------------------------------------------------------------
+inline int ConvexHull3D::GetType() const
+{
+ return m_iHullType;
+}
+//----------------------------------------------------------------------------
+inline int ConvexHull3D::GetQuantity() const
+{
+ return m_iHQuantity;
+}
+//----------------------------------------------------------------------------
+inline const int *ConvexHull3D::GetIndices() const
+{
+ return m_aiHIndex;
+}
+//----------------------------------------------------------------------------
+inline Real &ConvexHull3D::VertexEqualityEpsilon()
+{
+ return ms_fVertexEqualityEpsilon;
+}
+//----------------------------------------------------------------------------
+inline Real &ConvexHull3D::CollinearEpsilon()
+{
+ return ms_fCollinearEpsilon;
+}
+//----------------------------------------------------------------------------
+inline Real &ConvexHull3D::CoplanarEpsilon()
+{
+ return ms_fCoplanarEpsilon;
+}
+//----------------------------------------------------------------------------
diff --git a/src/editors/FreeMagic/MgcCylinder.h b/src/editors/FreeMagic/MgcCylinder.h
new file mode 100644
index 00000000000..e2c3d58a9e6
--- /dev/null
+++ b/src/editors/FreeMagic/MgcCylinder.h
@@ -0,0 +1,74 @@
+// Magic Software, Inc.
+// http://www.magic-software.com
+// Copyright (c) 2000-2002. All Rights Reserved
+//
+// Source code from Magic Software is supplied under the terms of a license
+// agreement and may not be copied or disclosed except in accordance with the
+// terms of that agreement. The various license agreements may be found at
+// the Magic Software web site. This file is subject to the license
+//
+// FREE SOURCE CODE
+// http://www.magic-software.com/License/free.pdf
+
+#ifndef MGCCYLINDER_H
+#define MGCCYLINDER_H
+
+#include "MgcSegment3.h"
+
+namespace Mgc
+{
+
+ class MAGICFM Cylinder
+ {
+ public:
+ // Cylinder line segment has end points C-(H/2)*D and C+(H/2)*D where
+ // D is a unit-length vector. H is infinity for infinite cylinder.
+
+ Cylinder();
+
+ Vector3 &Center();
+ const Vector3 &Center() const;
+
+ Vector3 &Direction();
+ const Vector3 &Direction() const;
+
+ Real &Height();
+ Real Height() const;
+
+ Real &Radius();
+ Real Radius() const;
+
+ // A value of 'true' means the cylinder caps (the end disks) are included
+ // as part of the cylinder. A value of 'false' means treat the cylinder
+ // as hollow--the end disks are not part of the object.
+ bool &Capped();
+ bool Capped() const;
+
+ Segment3 GetSegment() const;
+
+ // Call this function to generate a coordinate system for the cylinder,
+ // {U,V,W}, an orthonormal set where W is the unit-length direction of
+ // the cylinder axis. This is necessary for cylinder-cylinder
+ // intersection testing to avoid creating U and V for every test.
+ void GenerateCoordinateSystem();
+ Vector3 &U();
+ const Vector3 &U() const;
+ Vector3 &V();
+ const Vector3 &V() const;
+ Vector3 &W();
+ const Vector3 &W() const;
+
+ protected:
+ Vector3 m_kCenter;
+ Vector3 m_kDirection; // W
+ Vector3 m_kU, m_kV; // U, V
+ Real m_fHeight;
+ Real m_fRadius;
+ bool m_bCapped;
+ };
+
+#include "MgcCylinder.inl"
+
+} // namespace Mgc
+
+#endif
diff --git a/src/editors/FreeMagic/MgcCylinder.inl b/src/editors/FreeMagic/MgcCylinder.inl
new file mode 100644
index 00000000000..f60218ca68c
--- /dev/null
+++ b/src/editors/FreeMagic/MgcCylinder.inl
@@ -0,0 +1,111 @@
+// Magic Software, Inc.
+// http://www.magic-software.com
+// Copyright (c) 2000-2002. All Rights Reserved
+//
+// Source code from Magic Software is supplied under the terms of a license
+// agreement and may not be copied or disclosed except in accordance with the
+// terms of that agreement. The various license agreements may be found at
+// the Magic Software web site. This file is subject to the license
+//
+// FREE SOURCE CODE
+// http://www.magic-software.com/License/free.pdf
+
+//----------------------------------------------------------------------------
+inline Cylinder::Cylinder()
+{
+ // no initialization for efficiency
+}
+//----------------------------------------------------------------------------
+inline Vector3 &Cylinder::Center()
+{
+ return m_kCenter;
+}
+//----------------------------------------------------------------------------
+inline const Vector3 &Cylinder::Center() const
+{
+ return m_kCenter;
+}
+//----------------------------------------------------------------------------
+inline Vector3 &Cylinder::Direction()
+{
+ return m_kDirection;
+}
+//----------------------------------------------------------------------------
+inline const Vector3 &Cylinder::Direction() const
+{
+ return m_kDirection;
+}
+//----------------------------------------------------------------------------
+inline Real &Cylinder::Height()
+{
+ return m_fHeight;
+}
+//----------------------------------------------------------------------------
+inline Real Cylinder::Height() const
+{
+ return m_fHeight;
+}
+//----------------------------------------------------------------------------
+inline Real &Cylinder::Radius()
+{
+ return m_fRadius;
+}
+//----------------------------------------------------------------------------
+inline Real Cylinder::Radius() const
+{
+ return m_fRadius;
+}
+//----------------------------------------------------------------------------
+inline bool &Cylinder::Capped()
+{
+ return m_bCapped;
+}
+//----------------------------------------------------------------------------
+inline bool Cylinder::Capped() const
+{
+ return m_bCapped;
+}
+//----------------------------------------------------------------------------
+inline Segment3 Cylinder::GetSegment() const
+{
+ Segment3 kSegment;
+ kSegment.Direction() = m_fHeight * m_kDirection;
+ kSegment.Origin() = m_kCenter - 0.5 * kSegment.Direction();
+ return kSegment;
+}
+//----------------------------------------------------------------------------
+inline void Cylinder::GenerateCoordinateSystem()
+{
+ Vector3::GenerateOrthonormalBasis(m_kU, m_kV, m_kDirection, true);
+}
+//----------------------------------------------------------------------------
+inline Vector3 &Cylinder::U()
+{
+ return m_kU;
+}
+//----------------------------------------------------------------------------
+inline const Vector3 &Cylinder::U() const
+{
+ return m_kU;
+}
+//----------------------------------------------------------------------------
+inline Vector3 &Cylinder::V()
+{
+ return m_kV;
+}
+//----------------------------------------------------------------------------
+inline const Vector3 &Cylinder::V() const
+{
+ return m_kV;
+}
+//----------------------------------------------------------------------------
+inline Vector3 &Cylinder::W()
+{
+ return m_kDirection;
+}
+//----------------------------------------------------------------------------
+inline const Vector3 &Cylinder::W() const
+{
+ return m_kDirection;
+}
+//----------------------------------------------------------------------------
diff --git a/src/editors/FreeMagic/MgcDist3DVecLin.cpp b/src/editors/FreeMagic/MgcDist3DVecLin.cpp
new file mode 100644
index 00000000000..2d56ee02f7f
--- /dev/null
+++ b/src/editors/FreeMagic/MgcDist3DVecLin.cpp
@@ -0,0 +1,101 @@
+// Magic Software, Inc.
+// http://www.magic-software.com
+// Copyright (c) 2000-2002. All Rights Reserved
+//
+// Source code from Magic Software is supplied under the terms of a license
+// agreement and may not be copied or disclosed except in accordance with the
+// terms of that agreement. The various license agreements may be found at
+// the Magic Software web site. This file is subject to the license
+//
+// FREE SOURCE CODE
+// http://www.magic-software.com/License/free.pdf
+
+#include "MgcDist3DVecLin.h"
+using namespace Mgc;
+
+//----------------------------------------------------------------------------
+Real Mgc::SqrDistance(const Vector3 &rkPoint, const Line3 &rkLine,
+ Real *pfParam)
+{
+ Vector3 kDiff = rkPoint - rkLine.Origin();
+ Real fSqrLen = rkLine.Direction().SquaredLength();
+ Real fT = kDiff.Dot(rkLine.Direction()) / fSqrLen;
+ kDiff -= fT * rkLine.Direction();
+
+ if (pfParam)
+ *pfParam = fT;
+
+ return kDiff.SquaredLength();
+}
+//----------------------------------------------------------------------------
+Real Mgc::SqrDistance(const Vector3 &rkPoint, const Ray3 &rkRay,
+ Real *pfParam)
+{
+ Vector3 kDiff = rkPoint - rkRay.Origin();
+ Real fT = kDiff.Dot(rkRay.Direction());
+
+ if (fT <= 0.0f)
+ {
+ fT = 0.0f;
+ }
+ else
+ {
+ fT /= rkRay.Direction().SquaredLength();
+ kDiff -= fT * rkRay.Direction();
+ }
+
+ if (pfParam)
+ *pfParam = fT;
+
+ return kDiff.SquaredLength();
+}
+//----------------------------------------------------------------------------
+Real Mgc::SqrDistance(const Vector3 &rkPoint, const Segment3 &rkSegment,
+ Real *pfParam)
+{
+ Vector3 kDiff = rkPoint - rkSegment.Origin();
+ Real fT = kDiff.Dot(rkSegment.Direction());
+
+ if (fT <= 0.0f)
+ {
+ fT = 0.0f;
+ }
+ else
+ {
+ Real fSqrLen = rkSegment.Direction().SquaredLength();
+ if (fT >= fSqrLen)
+ {
+ fT = 1.0f;
+ kDiff -= rkSegment.Direction();
+ }
+ else
+ {
+ fT /= fSqrLen;
+ kDiff -= fT * rkSegment.Direction();
+ }
+ }
+
+ if (pfParam)
+ *pfParam = fT;
+
+ return kDiff.SquaredLength();
+}
+//----------------------------------------------------------------------------
+Real Mgc::Distance(const Vector3 &rkPoint, const Line3 &rkLine,
+ Real *pfParam)
+{
+ return Math::Sqrt(SqrDistance(rkPoint, rkLine, pfParam));
+}
+//----------------------------------------------------------------------------
+Real Mgc::Distance(const Vector3 &rkPoint, const Ray3 &rkRay,
+ Real *pfParam)
+{
+ return Math::Sqrt(SqrDistance(rkPoint, rkRay, pfParam));
+}
+//----------------------------------------------------------------------------
+Real Mgc::Distance(const Vector3 &rkPoint, const Segment3 &rkSegment,
+ Real *pfParam)
+{
+ return Math::Sqrt(SqrDistance(rkPoint, rkSegment, pfParam));
+}
+//----------------------------------------------------------------------------
diff --git a/src/editors/FreeMagic/MgcDist3DVecLin.h b/src/editors/FreeMagic/MgcDist3DVecLin.h
new file mode 100644
index 00000000000..d75eb87b933
--- /dev/null
+++ b/src/editors/FreeMagic/MgcDist3DVecLin.h
@@ -0,0 +1,47 @@
+// Magic Software, Inc.
+// http://www.magic-software.com
+// Copyright (c) 2000-2002. All Rights Reserved
+//
+// Source code from Magic Software is supplied under the terms of a license
+// agreement and may not be copied or disclosed except in accordance with the
+// terms of that agreement. The various license agreements may be found at
+// the Magic Software web site. This file is subject to the license
+//
+// FREE SOURCE CODE
+// http://www.magic-software.com/License/free.pdf
+
+#ifndef MGCDIST3DVECLIN_H
+#define MGCDIST3DVECLIN_H
+
+#include "MgcLine3.h"
+#include "MgcRay3.h"
+#include "MgcSegment3.h"
+
+namespace Mgc
+{
+
+ // squared distance measurements
+
+ MAGICFM Real SqrDistance(const Vector3 &rkPoint, const Line3 &rkLine,
+ Real *pfParam = NULL);
+
+ MAGICFM Real SqrDistance(const Vector3 &rkPoint, const Ray3 &rkRay,
+ Real *pfParam = NULL);
+
+ MAGICFM Real SqrDistance(const Vector3 &rkPoint, const Segment3 &rkSegment,
+ Real *pfParam = NULL);
+
+ // distance measurements
+
+ MAGICFM Real Distance(const Vector3 &rkPoint, const Line3 &rkLine,
+ Real *pfParam = NULL);
+
+ MAGICFM Real Distance(const Vector3 &rkPoint, const Ray3 &rkRay,
+ Real *pfParam = NULL);
+
+ MAGICFM Real Distance(const Vector3 &rkPoint, const Segment3 &rkSegment,
+ Real *pfParam = NULL);
+
+} // namespace Mgc
+
+#endif
diff --git a/src/editors/FreeMagic/MgcEigen.cpp b/src/editors/FreeMagic/MgcEigen.cpp
new file mode 100644
index 00000000000..ccdbedafee0
--- /dev/null
+++ b/src/editors/FreeMagic/MgcEigen.cpp
@@ -0,0 +1,669 @@
+// Magic Software, Inc.
+// http://www.magic-software.com
+// Copyright (c) 2000-2002. All Rights Reserved
+//
+// Source code from Magic Software is supplied under the terms of a license
+// agreement and may not be copied or disclosed except in accordance with the
+// terms of that agreement. The various license agreements may be found at
+// the Magic Software web site. This file is subject to the license
+//
+// FREE SOURCE CODE
+// http://www.magic-software.com/License/free.pdf
+//#include "stdafx.h"
+#pragma hdrstop
+
+#include "MgcEigen.h"
+using namespace Mgc;
+
+//----------------------------------------------------------------------------
+Eigen::Eigen(int iSize)
+{
+ assert(iSize >= 2);
+ m_iSize = iSize;
+
+ m_aafMat = new Real *[m_iSize];
+ for (int i = 0; i < m_iSize; i++)
+ m_aafMat[i] = new Real[m_iSize];
+
+ m_afDiag = new Real[m_iSize];
+ m_afSubd = new Real[m_iSize];
+}
+//----------------------------------------------------------------------------
+Eigen::~Eigen()
+{
+ delete[] m_afSubd;
+ delete[] m_afDiag;
+ for (int i = 0; i < m_iSize; i++)
+ delete[] m_aafMat[i];
+ delete[] m_aafMat;
+}
+//----------------------------------------------------------------------------
+void Eigen::Tridiagonal2(Real **m_aafMat, Real *m_afDiag, Real *m_afSubd)
+{
+ // matrix is already tridiagonal
+ m_afDiag[0] = m_aafMat[0][0];
+ m_afDiag[1] = m_aafMat[1][1];
+ m_afSubd[0] = m_aafMat[0][1];
+ m_afSubd[1] = 0.0f;
+ m_aafMat[0][0] = 1.0f;
+ m_aafMat[0][1] = 0.0f;
+ m_aafMat[1][0] = 0.0f;
+ m_aafMat[1][1] = 1.0f;
+}
+//----------------------------------------------------------------------------
+void Eigen::Tridiagonal3(Real **m_aafMat, Real *m_afDiag, Real *m_afSubd)
+{
+ Real fM00 = m_aafMat[0][0];
+ Real fM01 = m_aafMat[0][1];
+ Real fM02 = m_aafMat[0][2];
+ Real fM11 = m_aafMat[1][1];
+ Real fM12 = m_aafMat[1][2];
+ Real fM22 = m_aafMat[2][2];
+
+ m_afDiag[0] = fM00;
+ m_afSubd[2] = 0.0f;
+ if (fM02 != 0.0f)
+ {
+ Real fLength = Math::Sqrt(fM01 * fM01 + fM02 * fM02);
+ Real fInvLength = 1.0f / fLength;
+ fM01 *= fInvLength;
+ fM02 *= fInvLength;
+ Real fQ = 2.0f * fM01 * fM12 + fM02 * (fM22 - fM11);
+ m_afDiag[1] = fM11 + fM02 * fQ;
+ m_afDiag[2] = fM22 - fM02 * fQ;
+ m_afSubd[0] = fLength;
+ m_afSubd[1] = fM12 - fM01 * fQ;
+ m_aafMat[0][0] = 1.0f;
+ m_aafMat[0][1] = 0.0f;
+ m_aafMat[0][2] = 0.0f;
+ m_aafMat[1][0] = 0.0f;
+ m_aafMat[1][1] = fM01;
+ m_aafMat[1][2] = fM02;
+ m_aafMat[2][0] = 0.0f;
+ m_aafMat[2][1] = fM02;
+ m_aafMat[2][2] = -fM01;
+ }
+ else
+ {
+ m_afDiag[1] = fM11;
+ m_afDiag[2] = fM22;
+ m_afSubd[0] = fM01;
+ m_afSubd[1] = fM12;
+ m_aafMat[0][0] = 1.0f;
+ m_aafMat[0][1] = 0.0f;
+ m_aafMat[0][2] = 0.0f;
+ m_aafMat[1][0] = 0.0f;
+ m_aafMat[1][1] = 1.0f;
+ m_aafMat[1][2] = 0.0f;
+ m_aafMat[2][0] = 0.0f;
+ m_aafMat[2][1] = 0.0f;
+ m_aafMat[2][2] = 1.0f;
+ }
+}
+//----------------------------------------------------------------------------
+void Eigen::Tridiagonal4(Real **m_aafMat, Real *m_afDiag, Real *m_afSubd)
+{
+ // save matrix M
+ Real fM00 = m_aafMat[0][0];
+ Real fM01 = m_aafMat[0][1];
+ Real fM02 = m_aafMat[0][2];
+ Real fM03 = m_aafMat[0][3];
+ Real fM11 = m_aafMat[1][1];
+ Real fM12 = m_aafMat[1][2];
+ Real fM13 = m_aafMat[1][3];
+ Real fM22 = m_aafMat[2][2];
+ Real fM23 = m_aafMat[2][3];
+ Real fM33 = m_aafMat[3][3];
+
+ m_afDiag[0] = fM00;
+ m_afSubd[3] = 0.0f;
+
+ m_aafMat[0][0] = 1.0f;
+ m_aafMat[0][1] = 0.0f;
+ m_aafMat[0][2] = 0.0f;
+ m_aafMat[0][3] = 0.0f;
+ m_aafMat[1][0] = 0.0f;
+ m_aafMat[2][0] = 0.0f;
+ m_aafMat[3][0] = 0.0f;
+
+ Real fLength, fInvLength;
+
+ if (fM02 != 0.0f || fM03 != 0.0f)
+ {
+ Real fQ11, fQ12, fQ13;
+ Real fQ21, fQ22, fQ23;
+ Real fQ31, fQ32, fQ33;
+
+ // build column Q1
+ fLength = Math::Sqrt(fM01 * fM01 + fM02 * fM02 + fM03 * fM03);
+ fInvLength = 1.0f / fLength;
+ fQ11 = fM01 * fInvLength;
+ fQ21 = fM02 * fInvLength;
+ fQ31 = fM03 * fInvLength;
+
+ m_afSubd[0] = fLength;
+
+ // compute S*Q1
+ Real fV0 = fM11 * fQ11 + fM12 * fQ21 + fM13 * fQ31;
+ Real fV1 = fM12 * fQ11 + fM22 * fQ21 + fM23 * fQ31;
+ Real fV2 = fM13 * fQ11 + fM23 * fQ21 + fM33 * fQ31;
+
+ m_afDiag[1] = fQ11 * fV0 + fQ21 * fV1 + fQ31 * fV2;
+
+ // build column Q3 = Q1x(S*Q1)
+ fQ13 = fQ21 * fV2 - fQ31 * fV1;
+ fQ23 = fQ31 * fV0 - fQ11 * fV2;
+ fQ33 = fQ11 * fV1 - fQ21 * fV0;
+ fLength = Math::Sqrt(fQ13 * fQ13 + fQ23 * fQ23 + fQ33 * fQ33);
+ if (fLength > 0.0f)
+ {
+ fInvLength = 1.0f / fLength;
+ fQ13 *= fInvLength;
+ fQ23 *= fInvLength;
+ fQ33 *= fInvLength;
+
+ // build column Q2 = Q3xQ1
+ fQ12 = fQ23 * fQ31 - fQ33 * fQ21;
+ fQ22 = fQ33 * fQ11 - fQ13 * fQ31;
+ fQ32 = fQ13 * fQ21 - fQ23 * fQ11;
+
+ fV0 = fQ12 * fM11 + fQ22 * fM12 + fQ32 * fM13;
+ fV1 = fQ12 * fM12 + fQ22 * fM22 + fQ32 * fM23;
+ fV2 = fQ12 * fM13 + fQ22 * fM23 + fQ32 * fM33;
+ m_afSubd[1] = fQ11 * fV0 + fQ21 * fV1 + fQ31 * fV2;
+ m_afDiag[2] = fQ12 * fV0 + fQ22 * fV1 + fQ32 * fV2;
+ m_afSubd[2] = fQ13 * fV0 + fQ23 * fV1 + fQ33 * fV2;
+
+ fV0 = fQ13 * fM11 + fQ23 * fM12 + fQ33 * fM13;
+ fV1 = fQ13 * fM12 + fQ23 * fM22 + fQ33 * fM23;
+ fV2 = fQ13 * fM13 + fQ23 * fM23 + fQ33 * fM33;
+ m_afDiag[3] = fQ13 * fV0 + fQ23 * fV1 + fQ33 * fV2;
+ }
+ else
+ {
+ // S*Q1 parallel to Q1, choose any valid Q2 and Q3
+ m_afSubd[1] = 0.0f;
+
+ fLength = fQ21 * fQ21 + fQ31 * fQ31;
+ if (fLength > 0.0f)
+ {
+ fInvLength = 1.0f / fLength;
+ Real fTmp = fQ11 - 1.0f;
+ fQ12 = -fQ21;
+ fQ22 = 1.0f + fTmp * fQ21 * fQ21 * fInvLength;
+ fQ32 = fTmp * fQ21 * fQ31 * fInvLength;
+
+ fQ13 = -fQ31;
+ fQ23 = fQ32;
+ fQ33 = 1.0f + fTmp * fQ31 * fQ31 * fInvLength;
+
+ fV0 = fQ12 * fM11 + fQ22 * fM12 + fQ32 * fM13;
+ fV1 = fQ12 * fM12 + fQ22 * fM22 + fQ32 * fM23;
+ fV2 = fQ12 * fM13 + fQ22 * fM23 + fQ32 * fM33;
+ m_afDiag[2] = fQ12 * fV0 + fQ22 * fV1 + fQ32 * fV2;
+ m_afSubd[2] = fQ13 * fV0 + fQ23 * fV1 + fQ33 * fV2;
+
+ fV0 = fQ13 * fM11 + fQ23 * fM12 + fQ33 * fM13;
+ fV1 = fQ13 * fM12 + fQ23 * fM22 + fQ33 * fM23;
+ fV2 = fQ13 * fM13 + fQ23 * fM23 + fQ33 * fM33;
+ m_afDiag[3] = fQ13 * fV0 + fQ23 * fV1 + fQ33 * fV2;
+ }
+ else
+ {
+ // Q1 = (+-1,0,0)
+ fQ12 = 0.0f;
+ fQ22 = 1.0f;
+ fQ32 = 0.0f;
+ fQ13 = 0.0f;
+ fQ23 = 0.0f;
+ fQ33 = 1.0f;
+
+ m_afDiag[2] = fM22;
+ m_afDiag[3] = fM33;
+ m_afSubd[2] = fM23;
+ }
+ }
+
+ m_aafMat[1][1] = fQ11;
+ m_aafMat[1][2] = fQ12;
+ m_aafMat[1][3] = fQ13;
+ m_aafMat[2][1] = fQ21;
+ m_aafMat[2][2] = fQ22;
+ m_aafMat[2][3] = fQ23;
+ m_aafMat[3][1] = fQ31;
+ m_aafMat[3][2] = fQ32;
+ m_aafMat[3][3] = fQ33;
+ }
+ else
+ {
+ m_afDiag[1] = fM11;
+ m_afSubd[0] = fM01;
+ m_aafMat[1][1] = 1.0f;
+ m_aafMat[2][1] = 0.0f;
+ m_aafMat[3][1] = 0.0f;
+
+ if (fM13 != 0.0f)
+ {
+ fLength = Math::Sqrt(fM12 * fM12 + fM13 * fM13);
+ fInvLength = 1.0f / fLength;
+ fM12 *= fInvLength;
+ fM13 *= fInvLength;
+ Real fQ = 2.0f * fM12 * fM23 + fM13 * (fM33 - fM22);
+
+ m_afDiag[2] = fM22 + fM13 * fQ;
+ m_afDiag[3] = fM33 - fM13 * fQ;
+ m_afSubd[1] = fLength;
+ m_afSubd[2] = fM23 - fM12 * fQ;
+ m_aafMat[1][2] = 0.0f;
+ m_aafMat[1][3] = 0.0f;
+ m_aafMat[2][2] = fM12;
+ m_aafMat[2][3] = fM13;
+ m_aafMat[3][2] = fM13;
+ m_aafMat[3][3] = -fM12;
+ }
+ else
+ {
+ m_afDiag[2] = fM22;
+ m_afDiag[3] = fM33;
+ m_afSubd[1] = fM12;
+ m_afSubd[2] = fM23;
+ m_aafMat[1][2] = 0.0f;
+ m_aafMat[1][3] = 0.0f;
+ m_aafMat[2][2] = 1.0f;
+ m_aafMat[2][3] = 0.0f;
+ m_aafMat[3][2] = 0.0f;
+ m_aafMat[3][3] = 1.0f;
+ }
+ }
+}
+//----------------------------------------------------------------------------
+void Eigen::TridiagonalN(int iSize, Real **m_aafMat, Real *m_afDiag,
+ Real *m_afSubd)
+{
+ int i0, i1, i2, i3;
+
+ for (i0 = iSize - 1, i3 = iSize - 2; i0 >= 1; i0--, i3--)
+ {
+ Real fH = 0.0f, fScale = 0.0f;
+
+ if (i3 > 0)
+ {
+ for (i2 = 0; i2 <= i3; i2++)
+ fScale += Math::FAbs(m_aafMat[i0][i2]);
+ if (fScale == 0.0f)
+ {
+ m_afSubd[i0] = m_aafMat[i0][i3];
+ }
+ else
+ {
+ Real fInvScale = 1.0f / fScale;
+ for (i2 = 0; i2 <= i3; i2++)
+ {
+ m_aafMat[i0][i2] *= fInvScale;
+ fH += m_aafMat[i0][i2] * m_aafMat[i0][i2];
+ }
+ Real fF = m_aafMat[i0][i3];
+ Real fG = Math::Sqrt(fH);
+ if (fF > 0.0f)
+ fG = -fG;
+ m_afSubd[i0] = fScale * fG;
+ fH -= fF * fG;
+ m_aafMat[i0][i3] = fF - fG;
+ fF = 0.0f;
+ Real fInvH = 1.0f / fH;
+ for (i1 = 0; i1 <= i3; i1++)
+ {
+ m_aafMat[i1][i0] = m_aafMat[i0][i1] * fInvH;
+ fG = 0.0f;
+ for (i2 = 0; i2 <= i1; i2++)
+ fG += m_aafMat[i1][i2] * m_aafMat[i0][i2];
+ for (i2 = i1 + 1; i2 <= i3; i2++)
+ fG += m_aafMat[i2][i1] * m_aafMat[i0][i2];
+ m_afSubd[i1] = fG * fInvH;
+ fF += m_afSubd[i1] * m_aafMat[i0][i1];
+ }
+ Real fHalfFdivH = 0.5f * fF * fInvH;
+ for (i1 = 0; i1 <= i3; i1++)
+ {
+ fF = m_aafMat[i0][i1];
+ fG = m_afSubd[i1] - fHalfFdivH * fF;
+ m_afSubd[i1] = fG;
+ for (i2 = 0; i2 <= i1; i2++)
+ {
+ m_aafMat[i1][i2] -= fF * m_afSubd[i2] +
+ fG * m_aafMat[i0][i2];
+ }
+ }
+ }
+ }
+ else
+ {
+ m_afSubd[i0] = m_aafMat[i0][i3];
+ }
+
+ m_afDiag[i0] = fH;
+ }
+
+ m_afDiag[0] = 0.0f;
+ m_afSubd[0] = 0.0f;
+ for (i0 = 0, i3 = -1; i0 <= iSize - 1; i0++, i3++)
+ {
+ if (m_afDiag[i0] != 0.0f)
+ {
+ for (i1 = 0; i1 <= i3; i1++)
+ {
+ Real fSum = 0.0f;
+ for (i2 = 0; i2 <= i3; i2++)
+ fSum += m_aafMat[i0][i2] * m_aafMat[i2][i1];
+ for (i2 = 0; i2 <= i3; i2++)
+ m_aafMat[i2][i1] -= fSum * m_aafMat[i2][i0];
+ }
+ }
+ m_afDiag[i0] = m_aafMat[i0][i0];
+ m_aafMat[i0][i0] = 1.0f;
+ for (i1 = 0; i1 <= i3; i1++)
+ {
+ m_aafMat[i1][i0] = 0.0f;
+ m_aafMat[i0][i1] = 0.0f;
+ }
+ }
+
+ // re-ordering if Eigen::QLAlgorithm is used subsequently
+ for (i0 = 1, i3 = 0; i0 < iSize; i0++, i3++)
+ m_afSubd[i3] = m_afSubd[i0];
+ m_afSubd[iSize - 1] = 0.0f;
+}
+//----------------------------------------------------------------------------
+bool Eigen::QLAlgorithm(int iSize, Real *m_afDiag, Real *m_afSubd,
+ Real **m_aafMat)
+{
+ const int iMaxIter = 32;
+
+ for (int i0 = 0; i0 < iSize; i0++)
+ {
+ int i1;
+ for (i1 = 0; i1 < iMaxIter; i1++)
+ {
+ int i2;
+ for (i2 = i0; i2 <= iSize - 2; i2++)
+ {
+ Real fTmp = Math::FAbs(m_afDiag[i2]) +
+ Math::FAbs(m_afDiag[i2 + 1]);
+ if (Math::FAbs(m_afSubd[i2]) + fTmp == fTmp)
+ break;
+ }
+ if (i2 == i0)
+ break;
+
+ Real fG = (m_afDiag[i0 + 1] - m_afDiag[i0]) / (2.0f * m_afSubd[i0]);
+ Real fR = Math::Sqrt(fG * fG + 1.0f);
+ if (fG < 0.0f)
+ fG = m_afDiag[i2] - m_afDiag[i0] + m_afSubd[i0] / (fG - fR);
+ else
+ fG = m_afDiag[i2] - m_afDiag[i0] + m_afSubd[i0] / (fG + fR);
+ Real fSin = 1.0f, fCos = 1.0f, fP = 0.0f;
+ for (int i3 = i2 - 1; i3 >= i0; i3--)
+ {
+ Real fF = fSin * m_afSubd[i3];
+ Real fB = fCos * m_afSubd[i3];
+ if (Math::FAbs(fF) >= Math::FAbs(fG))
+ {
+ fCos = fG / fF;
+ fR = Math::Sqrt(fCos * fCos + 1.0f);
+ m_afSubd[i3 + 1] = fF * fR;
+ fSin = 1.0f / fR;
+ fCos *= fSin;
+ }
+ else
+ {
+ fSin = fF / fG;
+ fR = Math::Sqrt(fSin * fSin + 1.0f);
+ m_afSubd[i3 + 1] = fG * fR;
+ fCos = 1.0f / fR;
+ fSin *= fCos;
+ }
+ fG = m_afDiag[i3 + 1] - fP;
+ fR = (m_afDiag[i3] - fG) * fSin + 2.0f * fB * fCos;
+ fP = fSin * fR;
+ m_afDiag[i3 + 1] = fG + fP;
+ fG = fCos * fR - fB;
+
+ for (int i4 = 0; i4 < iSize; i4++)
+ {
+ fF = m_aafMat[i4][i3 + 1];
+ m_aafMat[i4][i3 + 1] = fSin * m_aafMat[i4][i3] + fCos * fF;
+ m_aafMat[i4][i3] = fCos * m_aafMat[i4][i3] - fSin * fF;
+ }
+ }
+ m_afDiag[i0] -= fP;
+ m_afSubd[i0] = fG;
+ m_afSubd[i2] = 0.0f;
+ }
+ if (i1 == iMaxIter)
+ return false;
+ }
+
+ return true;
+}
+//----------------------------------------------------------------------------
+void Eigen::DecreasingSort(int iSize, Real *afEigval, Real **aafEigvec)
+{
+ // sort eigenvalues in decreasing order, e[0] >= ... >= e[iSize-1]
+ for (int i0 = 0, i1; i0 <= iSize - 2; i0++)
+ {
+ // locate maximum eigenvalue
+ i1 = i0;
+ Real fMax = afEigval[i1];
+ int i2;
+ for (i2 = i0 + 1; i2 < iSize; i2++)
+ {
+ if (afEigval[i2] > fMax)
+ {
+ i1 = i2;
+ fMax = afEigval[i1];
+ }
+ }
+
+ if (i1 != i0)
+ {
+ // swap eigenvalues
+ afEigval[i1] = afEigval[i0];
+ afEigval[i0] = fMax;
+
+ // swap eigenvectors
+ for (i2 = 0; i2 < iSize; i2++)
+ {
+ Real fTmp = aafEigvec[i2][i0];
+ aafEigvec[i2][i0] = aafEigvec[i2][i1];
+ aafEigvec[i2][i1] = fTmp;
+ }
+ }
+ }
+}
+//----------------------------------------------------------------------------
+void Eigen::IncreasingSort(int iSize, Real *afEigval, Real **aafEigvec)
+{
+ // sort eigenvalues in increasing order, e[0] <= ... <= e[iSize-1]
+ for (int i0 = 0, i1; i0 <= iSize - 2; i0++)
+ {
+ // locate minimum eigenvalue
+ i1 = i0;
+ Real fMin = afEigval[i1];
+ int i2;
+ for (i2 = i0 + 1; i2 < iSize; i2++)
+ {
+ if (afEigval[i2] < fMin)
+ {
+ i1 = i2;
+ fMin = afEigval[i1];
+ }
+ }
+
+ if (i1 != i0)
+ {
+ // swap eigenvalues
+ afEigval[i1] = afEigval[i0];
+ afEigval[i0] = fMin;
+
+ // swap eigenvectors
+ for (i2 = 0; i2 < iSize; i2++)
+ {
+ Real fTmp = aafEigvec[i2][i0];
+ aafEigvec[i2][i0] = aafEigvec[i2][i1];
+ aafEigvec[i2][i1] = fTmp;
+ }
+ }
+ }
+}
+//----------------------------------------------------------------------------
+void Eigen::SetMatrix(Real **aafMat)
+{
+ for (int iRow = 0; iRow < m_iSize; iRow++)
+ {
+ for (int iCol = 0; iCol < m_iSize; iCol++)
+ m_aafMat[iRow][iCol] = aafMat[iRow][iCol];
+ }
+}
+//----------------------------------------------------------------------------
+void Eigen::EigenStuff2()
+{
+ Tridiagonal2(m_aafMat, m_afDiag, m_afSubd);
+ QLAlgorithm(m_iSize, m_afDiag, m_afSubd, m_aafMat);
+}
+//----------------------------------------------------------------------------
+void Eigen::EigenStuff3()
+{
+ Tridiagonal3(m_aafMat, m_afDiag, m_afSubd);
+ QLAlgorithm(m_iSize, m_afDiag, m_afSubd, m_aafMat);
+}
+//----------------------------------------------------------------------------
+void Eigen::EigenStuff4()
+{
+ Tridiagonal4(m_aafMat, m_afDiag, m_afSubd);
+ QLAlgorithm(m_iSize, m_afDiag, m_afSubd, m_aafMat);
+}
+//----------------------------------------------------------------------------
+void Eigen::EigenStuffN()
+{
+ TridiagonalN(m_iSize, m_aafMat, m_afDiag, m_afSubd);
+ QLAlgorithm(m_iSize, m_afDiag, m_afSubd, m_aafMat);
+}
+//----------------------------------------------------------------------------
+void Eigen::EigenStuff()
+{
+ switch (m_iSize)
+ {
+ case 2:
+ Tridiagonal2(m_aafMat, m_afDiag, m_afSubd);
+ break;
+ case 3:
+ Tridiagonal3(m_aafMat, m_afDiag, m_afSubd);
+ break;
+ case 4:
+ Tridiagonal4(m_aafMat, m_afDiag, m_afSubd);
+ break;
+ default:
+ TridiagonalN(m_iSize, m_aafMat, m_afDiag, m_afSubd);
+ break;
+ }
+ QLAlgorithm(m_iSize, m_afDiag, m_afSubd, m_aafMat);
+}
+//----------------------------------------------------------------------------
+void Eigen::DecrSortEigenStuff2()
+{
+ Tridiagonal2(m_aafMat, m_afDiag, m_afSubd);
+ QLAlgorithm(m_iSize, m_afDiag, m_afSubd, m_aafMat);
+ DecreasingSort(m_iSize, m_afDiag, m_aafMat);
+}
+//----------------------------------------------------------------------------
+void Eigen::DecrSortEigenStuff3()
+{
+ Tridiagonal3(m_aafMat, m_afDiag, m_afSubd);
+ QLAlgorithm(m_iSize, m_afDiag, m_afSubd, m_aafMat);
+ DecreasingSort(m_iSize, m_afDiag, m_aafMat);
+}
+//----------------------------------------------------------------------------
+void Eigen::DecrSortEigenStuff4()
+{
+ Tridiagonal4(m_aafMat, m_afDiag, m_afSubd);
+ QLAlgorithm(m_iSize, m_afDiag, m_afSubd, m_aafMat);
+ DecreasingSort(m_iSize, m_afDiag, m_aafMat);
+}
+//----------------------------------------------------------------------------
+void Eigen::DecrSortEigenStuffN()
+{
+ TridiagonalN(m_iSize, m_aafMat, m_afDiag, m_afSubd);
+ QLAlgorithm(m_iSize, m_afDiag, m_afSubd, m_aafMat);
+ DecreasingSort(m_iSize, m_afDiag, m_aafMat);
+}
+//----------------------------------------------------------------------------
+void Eigen::DecrSortEigenStuff()
+{
+ switch (m_iSize)
+ {
+ case 2:
+ Tridiagonal2(m_aafMat, m_afDiag, m_afSubd);
+ break;
+ case 3:
+ Tridiagonal3(m_aafMat, m_afDiag, m_afSubd);
+ break;
+ case 4:
+ Tridiagonal4(m_aafMat, m_afDiag, m_afSubd);
+ break;
+ default:
+ TridiagonalN(m_iSize, m_aafMat, m_afDiag, m_afSubd);
+ break;
+ }
+ QLAlgorithm(m_iSize, m_afDiag, m_afSubd, m_aafMat);
+ DecreasingSort(m_iSize, m_afDiag, m_aafMat);
+}
+//----------------------------------------------------------------------------
+void Eigen::IncrSortEigenStuff2()
+{
+ Tridiagonal2(m_aafMat, m_afDiag, m_afSubd);
+ QLAlgorithm(m_iSize, m_afDiag, m_afSubd, m_aafMat);
+ IncreasingSort(m_iSize, m_afDiag, m_aafMat);
+}
+//----------------------------------------------------------------------------
+void Eigen::IncrSortEigenStuff3()
+{
+ Tridiagonal3(m_aafMat, m_afDiag, m_afSubd);
+ QLAlgorithm(m_iSize, m_afDiag, m_afSubd, m_aafMat);
+ IncreasingSort(m_iSize, m_afDiag, m_aafMat);
+}
+//----------------------------------------------------------------------------
+void Eigen::IncrSortEigenStuff4()
+{
+ Tridiagonal4(m_aafMat, m_afDiag, m_afSubd);
+ QLAlgorithm(m_iSize, m_afDiag, m_afSubd, m_aafMat);
+ IncreasingSort(m_iSize, m_afDiag, m_aafMat);
+}
+//----------------------------------------------------------------------------
+void Eigen::IncrSortEigenStuffN()
+{
+ TridiagonalN(m_iSize, m_aafMat, m_afDiag, m_afSubd);
+ QLAlgorithm(m_iSize, m_afDiag, m_afSubd, m_aafMat);
+ IncreasingSort(m_iSize, m_afDiag, m_aafMat);
+}
+//----------------------------------------------------------------------------
+void Eigen::IncrSortEigenStuff()
+{
+ switch (m_iSize)
+ {
+ case 2:
+ Tridiagonal2(m_aafMat, m_afDiag, m_afSubd);
+ break;
+ case 3:
+ Tridiagonal3(m_aafMat, m_afDiag, m_afSubd);
+ break;
+ case 4:
+ Tridiagonal4(m_aafMat, m_afDiag, m_afSubd);
+ break;
+ default:
+ TridiagonalN(m_iSize, m_aafMat, m_afDiag, m_afSubd);
+ break;
+ }
+ QLAlgorithm(m_iSize, m_afDiag, m_afSubd, m_aafMat);
+ IncreasingSort(m_iSize, m_afDiag, m_aafMat);
+}
+//----------------------------------------------------------------------------
diff --git a/src/editors/FreeMagic/MgcEigen.h b/src/editors/FreeMagic/MgcEigen.h
new file mode 100644
index 00000000000..4b119abaf87
--- /dev/null
+++ b/src/editors/FreeMagic/MgcEigen.h
@@ -0,0 +1,88 @@
+// Magic Software, Inc.
+// http://www.magic-software.com
+// Copyright (c) 2000-2002. All Rights Reserved
+//
+// Source code from Magic Software is supplied under the terms of a license
+// agreement and may not be copied or disclosed except in accordance with the
+// terms of that agreement. The various license agreements may be found at
+// the Magic Software web site. This file is subject to the license
+//
+// FREE SOURCE CODE
+// http://www.magic-software.com/License/free.pdf
+
+#ifndef MGCEIGEN_H
+#define MGCEIGEN_H
+
+#include "MgcMath.h"
+
+namespace Mgc
+{
+
+ class MAGICFM Eigen
+ {
+ public:
+ Eigen(int iSize);
+ ~Eigen();
+
+ // set the matrix for eigensolving
+ Real &Matrix(int iRow, int iCol);
+ void SetMatrix(Real **aafMat);
+
+ // get the results of eigensolving (eigenvectors are columns of matrix)
+ Real GetEigenvalue(int i) const;
+ Real GetEigenvector(int iRow, int iCol) const;
+ Real *GetEigenvalue();
+ Real **GetEigenvector();
+
+ // solve eigensystem
+ void EigenStuff2();
+ void EigenStuff3();
+ void EigenStuff4();
+ void EigenStuffN();
+ void EigenStuff();
+
+ // solve eigensystem, use decreasing sort on eigenvalues
+ void DecrSortEigenStuff2();
+ void DecrSortEigenStuff3();
+ void DecrSortEigenStuff4();
+ void DecrSortEigenStuffN();
+ void DecrSortEigenStuff();
+
+ // solve eigensystem, use increasing sort on eigenvalues
+ void IncrSortEigenStuff2();
+ void IncrSortEigenStuff3();
+ void IncrSortEigenStuff4();
+ void IncrSortEigenStuffN();
+ void IncrSortEigenStuff();
+
+ protected:
+ int m_iSize;
+ Real **m_aafMat;
+ Real *m_afDiag;
+ Real *m_afSubd;
+
+ // Householder reduction to tridiagonal form
+ static void Tridiagonal2(Real **aafMat, Real *afDiag, Real *afSubd);
+ static void Tridiagonal3(Real **aafMat, Real *afDiag, Real *afSubd);
+ static void Tridiagonal4(Real **aafMat, Real *afDiag, Real *afSubd);
+ static void TridiagonalN(int iSize, Real **aafMat, Real *afDiag,
+ Real *afSubd);
+
+ // QL algorithm with implicit shifting, applies to tridiagonal matrices
+ static bool QLAlgorithm(int iSize, Real *afDiag, Real *afSubd,
+ Real **aafMat);
+
+ // sort eigenvalues from largest to smallest
+ static void DecreasingSort(int iSize, Real *afEigval,
+ Real **aafEigvec);
+
+ // sort eigenvalues from smallest to largest
+ static void IncreasingSort(int iSize, Real *afEigval,
+ Real **aafEigvec);
+ };
+
+#include "MgcEigen.inl"
+
+} // namespace Mgc
+
+#endif
diff --git a/src/editors/FreeMagic/MgcEigen.inl b/src/editors/FreeMagic/MgcEigen.inl
new file mode 100644
index 00000000000..d8d246e56eb
--- /dev/null
+++ b/src/editors/FreeMagic/MgcEigen.inl
@@ -0,0 +1,38 @@
+// Magic Software, Inc.
+// http://www.magic-software.com
+// Copyright (c) 2000-2002. All Rights Reserved
+//
+// Source code from Magic Software is supplied under the terms of a license
+// agreement and may not be copied or disclosed except in accordance with the
+// terms of that agreement. The various license agreements may be found at
+// the Magic Software web site. This file is subject to the license
+//
+// FREE SOURCE CODE
+// http://www.magic-software.com/License/free.pdf
+
+//----------------------------------------------------------------------------
+inline Real &Eigen::Matrix(int iRow, int iCol)
+{
+ return m_aafMat[iRow][iCol];
+}
+//----------------------------------------------------------------------------
+inline Real Eigen::GetEigenvalue(int i) const
+{
+ return m_afDiag[i];
+}
+//----------------------------------------------------------------------------
+inline Real Eigen::GetEigenvector(int iRow, int iCol) const
+{
+ return m_aafMat[iRow][iCol];
+}
+//----------------------------------------------------------------------------
+inline Real *Eigen::GetEigenvalue()
+{
+ return m_afDiag;
+}
+//----------------------------------------------------------------------------
+inline Real **Eigen::GetEigenvector()
+{
+ return m_aafMat;
+}
+//----------------------------------------------------------------------------
diff --git a/src/editors/FreeMagic/MgcLine3.h b/src/editors/FreeMagic/MgcLine3.h
new file mode 100644
index 00000000000..f1c3e640560
--- /dev/null
+++ b/src/editors/FreeMagic/MgcLine3.h
@@ -0,0 +1,43 @@
+// Magic Software, Inc.
+// http://www.magic-software.com
+// Copyright (c) 2000-2002. All Rights Reserved
+//
+// Source code from Magic Software is supplied under the terms of a license
+// agreement and may not be copied or disclosed except in accordance with the
+// terms of that agreement. The various license agreements may be found at
+// the Magic Software web site. This file is subject to the license
+//
+// FREE SOURCE CODE
+// http://www.magic-software.com/License/free.pdf
+
+#ifndef MGCLINE3_H
+#define MGCLINE3_H
+
+#include "MgcVector3.h"
+
+namespace Mgc
+{
+
+ class MAGICFM Line3
+ {
+ public:
+ // Line is L(t) = P+t*D for any real-valued t. D is not necessarily
+ // unit length.
+ Line3();
+
+ Vector3 &Origin();
+ const Vector3 &Origin() const;
+
+ Vector3 &Direction();
+ const Vector3 &Direction() const;
+
+ protected:
+ Vector3 m_kOrigin; // P
+ Vector3 m_kDirection; // D
+ };
+
+#include "MgcLine3.inl"
+
+} // namespace Mgc
+
+#endif
diff --git a/src/editors/FreeMagic/MgcLine3.inl b/src/editors/FreeMagic/MgcLine3.inl
new file mode 100644
index 00000000000..caec4748d46
--- /dev/null
+++ b/src/editors/FreeMagic/MgcLine3.inl
@@ -0,0 +1,38 @@
+// Magic Software, Inc.
+// http://www.magic-software.com
+// Copyright (c) 2000-2002. All Rights Reserved
+//
+// Source code from Magic Software is supplied under the terms of a license
+// agreement and may not be copied or disclosed except in accordance with the
+// terms of that agreement. The various license agreements may be found at
+// the Magic Software web site. This file is subject to the license
+//
+// FREE SOURCE CODE
+// http://www.magic-software.com/License/free.pdf
+
+//----------------------------------------------------------------------------
+inline Line3::Line3()
+{
+ // no initialization for efficiency
+}
+//----------------------------------------------------------------------------
+inline Vector3 &Line3::Origin()
+{
+ return m_kOrigin;
+}
+//----------------------------------------------------------------------------
+inline const Vector3 &Line3::Origin() const
+{
+ return m_kOrigin;
+}
+//----------------------------------------------------------------------------
+inline Vector3 &Line3::Direction()
+{
+ return m_kDirection;
+}
+//----------------------------------------------------------------------------
+inline const Vector3 &Line3::Direction() const
+{
+ return m_kDirection;
+}
+//----------------------------------------------------------------------------
diff --git a/src/editors/FreeMagic/MgcLinearSystem.cpp b/src/editors/FreeMagic/MgcLinearSystem.cpp
new file mode 100644
index 00000000000..182bcbf2f47
--- /dev/null
+++ b/src/editors/FreeMagic/MgcLinearSystem.cpp
@@ -0,0 +1,505 @@
+// Magic Software, Inc.
+// http://www.magic-software.com
+// Copyright (c) 2000-2002. All Rights Reserved
+//
+// Source code from Magic Software is supplied under the terms of a license
+// agreement and may not be copied or disclosed except in accordance with the
+// terms of that agreement. The various license agreements may be found at
+// the Magic Software web site. This file is subject to the license
+//
+// FREE SOURCE CODE
+// http://www.magic-software.com/License/free.pdf
+
+#include "MgcLinearSystem.h"
+using namespace Mgc;
+
+Real LinearSystem::ms_fTolerance = 1e-06f;
+
+//----------------------------------------------------------------------------
+Real **LinearSystem::NewMatrix(int iSize)
+{
+ Real **aafA = new Real *[iSize];
+ assert(aafA);
+
+ for (int iRow = 0; iRow < iSize; iRow++)
+ {
+ aafA[iRow] = new Real[iSize];
+ memset(aafA[iRow], 0, iSize * sizeof(Real));
+ }
+
+ return aafA;
+}
+//----------------------------------------------------------------------------
+void LinearSystem::DeleteMatrix(int iSize, Real **aafA)
+{
+ for (int iRow = 0; iRow < iSize; iRow++)
+ delete[] aafA[iRow];
+ delete[] aafA;
+}
+//----------------------------------------------------------------------------
+Real *LinearSystem::NewVector(int iSize)
+{
+ Real *afB = new Real[iSize];
+ assert(afB);
+ memset(afB, 0, iSize * sizeof(Real));
+ return afB;
+}
+//----------------------------------------------------------------------------
+void LinearSystem::DeleteVector(Real *afB)
+{
+ delete[] afB;
+}
+//----------------------------------------------------------------------------
+bool LinearSystem::Solve2(const Real aafA[2][2], const Real afB[2],
+ Real afX[2])
+{
+ Real fDet = aafA[0][0] * aafA[1][1] - aafA[0][1] * aafA[1][0];
+ if (Math::FAbs(fDet) < ms_fTolerance)
+ return false;
+
+ Real fInvDet = 1.0f / fDet;
+ afX[0] = (aafA[1][1] * afB[0] - aafA[0][1] * afB[1]) * fInvDet;
+ afX[1] = (aafA[0][0] * afB[1] - aafA[1][0] * afB[0]) * fInvDet;
+ return true;
+}
+//----------------------------------------------------------------------------
+bool LinearSystem::Solve3(const Real aafA[3][3], const Real afB[3],
+ Real afX[3])
+{
+ Real aafAInv[3][3];
+ aafAInv[0][0] = aafA[1][1] * aafA[2][2] - aafA[1][2] * aafA[2][1];
+ aafAInv[0][1] = aafA[0][2] * aafA[2][1] - aafA[0][1] * aafA[2][2];
+ aafAInv[0][2] = aafA[0][1] * aafA[1][2] - aafA[0][2] * aafA[1][1];
+ aafAInv[1][0] = aafA[1][2] * aafA[2][0] - aafA[1][0] * aafA[2][2];
+ aafAInv[1][1] = aafA[0][0] * aafA[2][2] - aafA[0][2] * aafA[2][0];
+ aafAInv[1][2] = aafA[0][2] * aafA[1][0] - aafA[0][0] * aafA[1][2];
+ aafAInv[2][0] = aafA[1][0] * aafA[2][1] - aafA[1][1] * aafA[2][0];
+ aafAInv[2][1] = aafA[0][1] * aafA[2][0] - aafA[0][0] * aafA[2][1];
+ aafAInv[2][2] = aafA[0][0] * aafA[1][1] - aafA[0][1] * aafA[1][0];
+ Real fDet = aafA[0][0] * aafAInv[0][0] + aafA[0][1] * aafAInv[1][0] +
+ aafA[0][2] * aafAInv[2][0];
+ if (Math::FAbs(fDet) < ms_fTolerance)
+ return false;
+
+ Real fInvDet = 1.0f / fDet;
+ for (int iRow = 0; iRow < 3; iRow++)
+ {
+ for (int iCol = 0; iCol < 3; iCol++)
+ aafAInv[iRow][iCol] *= fInvDet;
+ }
+
+ afX[0] = aafAInv[0][0] * afB[0] + aafAInv[0][1] * afB[1] + aafAInv[0][2] * afB[2];
+ afX[1] = aafAInv[1][0] * afB[0] + aafAInv[1][1] * afB[1] + aafAInv[1][2] * afB[2];
+ afX[2] = aafAInv[2][0] * afB[0] + aafAInv[2][1] * afB[1] + aafAInv[2][2] * afB[2];
+ return true;
+}
+//----------------------------------------------------------------------------
+bool LinearSystem::Inverse(int iSize, Real **aafA)
+{
+ int *aiColIndex = new int[iSize];
+ assert(aiColIndex);
+
+ int *aiRowIndex = new int[iSize];
+ assert(aiRowIndex);
+
+ bool *abPivoted = new bool[iSize];
+ assert(abPivoted);
+ memset(abPivoted, 0, iSize * sizeof(bool));
+
+ int i1, i2, iRow, iCol;
+ Real fSave;
+
+ // elimination by full pivoting
+ for (int i0 = 0; i0 < iSize; i0++)
+ {
+ // search matrix (excluding pivoted rows) for maximum absolute entry
+ Real fMax = 0.0f;
+ for (i1 = 0; i1 < iSize; i1++)
+ {
+ if (!abPivoted[i1])
+ {
+ for (i2 = 0; i2 < iSize; i2++)
+ {
+ if (!abPivoted[i2])
+ {
+ Real fAbs = Math::FAbs(aafA[i1][i2]);
+ if (fAbs > fMax)
+ {
+ fMax = fAbs;
+ iRow = i1;
+ iCol = i2;
+ }
+ }
+ }
+ }
+ }
+
+ if (fMax == 0.0f)
+ {
+ // matrix is not invertible
+ delete[] aiColIndex;
+ delete[] aiRowIndex;
+ delete[] abPivoted;
+ return false;
+ }
+
+ abPivoted[iCol] = true;
+
+ // swap rows so that A[iCol][iCol] contains the pivot entry
+ if (iRow != iCol)
+ {
+ Real *afRowPtr = aafA[iRow];
+ aafA[iRow] = aafA[iCol];
+ aafA[iCol] = afRowPtr;
+ }
+
+ // keep track of the permutations of the rows
+ aiRowIndex[i0] = iRow;
+ aiColIndex[i0] = iCol;
+
+ // scale the row so that the pivot entry is 1
+ Real fInv = 1.0f / aafA[iCol][iCol];
+ aafA[iCol][iCol] = 1.0f;
+ for (i2 = 0; i2 < iSize; i2++)
+ aafA[iCol][i2] *= fInv;
+
+ // zero out the pivot column locations in the other rows
+ for (i1 = 0; i1 < iSize; i1++)
+ {
+ if (i1 != iCol)
+ {
+ fSave = aafA[i1][iCol];
+ aafA[i1][iCol] = 0.0f;
+ for (i2 = 0; i2 < iSize; i2++)
+ aafA[i1][i2] -= aafA[iCol][i2] * fSave;
+ }
+ }
+ }
+
+ // reorder rows so that A[][] stores the inverse of the original matrix
+ for (i1 = iSize - 1; i1 >= 0; i1--)
+ {
+ if (aiRowIndex[i1] != aiColIndex[i1])
+ {
+ for (i2 = 0; i2 < iSize; i2++)
+ {
+ fSave = aafA[i2][aiRowIndex[i1]];
+ aafA[i2][aiRowIndex[i1]] = aafA[i2][aiColIndex[i1]];
+ aafA[i2][aiColIndex[i1]] = fSave;
+ }
+ }
+ }
+
+ delete[] aiColIndex;
+ delete[] aiRowIndex;
+ delete[] abPivoted;
+ return true;
+}
+//----------------------------------------------------------------------------
+bool LinearSystem::Solve(int iSize, Real **aafA, Real *afB)
+{
+ int *aiColIndex = new int[iSize];
+ assert(aiColIndex);
+
+ int *aiRowIndex = new int[iSize];
+ assert(aiRowIndex);
+
+ bool *abPivoted = new bool[iSize];
+ assert(abPivoted);
+ memset(abPivoted, 0, iSize * sizeof(bool));
+
+ int i1, i2, iRow, iCol;
+ Real fSave;
+
+ // elimination by full pivoting
+ for (int i0 = 0; i0 < iSize; i0++)
+ {
+ // search matrix (excluding pivoted rows) for maximum absolute entry
+ Real fMax = 0.0f;
+ for (i1 = 0; i1 < iSize; i1++)
+ {
+ if (!abPivoted[i1])
+ {
+ for (i2 = 0; i2 < iSize; i2++)
+ {
+ if (!abPivoted[i2])
+ {
+ Real fAbs = Math::FAbs(aafA[i1][i2]);
+ if (fAbs > fMax)
+ {
+ fMax = fAbs;
+ iRow = i1;
+ iCol = i2;
+ }
+ }
+ }
+ }
+ }
+
+ if (fMax == 0.0f)
+ {
+ // matrix is not invertible
+ delete[] aiColIndex;
+ delete[] aiRowIndex;
+ delete[] abPivoted;
+ return false;
+ }
+
+ abPivoted[iCol] = true;
+
+ // swap rows so that A[iCol][iCol] contains the pivot entry
+ if (iRow != iCol)
+ {
+ Real *afRowPtr = aafA[iRow];
+ aafA[iRow] = aafA[iCol];
+ aafA[iCol] = afRowPtr;
+
+ fSave = afB[iRow];
+ afB[iRow] = afB[iCol];
+ afB[iCol] = fSave;
+ }
+
+ // keep track of the permutations of the rows
+ aiRowIndex[i0] = iRow;
+ aiColIndex[i0] = iCol;
+
+ // scale the row so that the pivot entry is 1
+ Real fInv = 1.0f / aafA[iCol][iCol];
+ aafA[iCol][iCol] = 1.0f;
+ for (i2 = 0; i2 < iSize; i2++)
+ aafA[iCol][i2] *= fInv;
+ afB[iCol] *= fInv;
+
+ // zero out the pivot column locations in the other rows
+ for (i1 = 0; i1 < iSize; i1++)
+ {
+ if (i1 != iCol)
+ {
+ fSave = aafA[i1][iCol];
+ aafA[i1][iCol] = 0.0f;
+ for (i2 = 0; i2 < iSize; i2++)
+ aafA[i1][i2] -= aafA[iCol][i2] * fSave;
+ afB[i1] -= afB[iCol] * fSave;
+ }
+ }
+ }
+
+ // reorder rows so that A[][] stores the inverse of the original matrix
+ for (i1 = iSize - 1; i1 >= 0; i1--)
+ {
+ if (aiRowIndex[i1] != aiColIndex[i1])
+ {
+ for (i2 = 0; i2 < iSize; i2++)
+ {
+ fSave = aafA[i2][aiRowIndex[i1]];
+ aafA[i2][aiRowIndex[i1]] = aafA[i2][aiColIndex[i1]];
+ aafA[i2][aiColIndex[i1]] = fSave;
+ }
+ }
+ }
+
+ delete[] aiColIndex;
+ delete[] aiRowIndex;
+ delete[] abPivoted;
+ return true;
+}
+//----------------------------------------------------------------------------
+bool LinearSystem::SolveTri(int iSize, Real *afA, Real *afB,
+ Real *afC, Real *afR, Real *afU)
+{
+ if (afB[0] == 0.0f)
+ return false;
+
+ Real *afD = new Real[iSize - 1];
+ assert(afD);
+
+ Real fE = afB[0];
+ Real fInvE = 1.0f / fE;
+ afU[0] = afR[0] * fInvE;
+
+ int i0, i1;
+ for (i0 = 0, i1 = 1; i1 < iSize; i0++, i1++)
+ {
+ afD[i0] = afC[i0] * fInvE;
+ fE = afB[i1] - afA[i0] * afD[i0];
+ if (fE == 0.0f)
+ {
+ delete[] afD;
+ return false;
+ }
+ fInvE = 1.0f / fE;
+ afU[i1] = (afR[i1] - afA[i0] * afU[i0]) * fInvE;
+ }
+
+ for (i0 = iSize - 1, i1 = iSize - 2; i1 >= 0; i0--, i1--)
+ afU[i1] -= afD[i1] * afU[i0];
+
+ delete[] afD;
+ return true;
+}
+//----------------------------------------------------------------------------
+bool LinearSystem::SolveConstTri(int iSize, Real fA, Real fB,
+ Real fC, Real *afR, Real *afU)
+{
+ if (fB == 0.0f)
+ return false;
+
+ Real *afD = new Real[iSize - 1];
+ assert(afD);
+
+ Real fE = fB;
+ Real fInvE = 1.0f / fE;
+ afU[0] = afR[0] * fInvE;
+
+ int i0, i1;
+ for (i0 = 0, i1 = 1; i1 < iSize; i0++, i1++)
+ {
+ afD[i0] = fC * fInvE;
+ fE = fB - fA * afD[i0];
+ if (fE == 0.0f)
+ {
+ delete[] afD;
+ return false;
+ }
+ fInvE = 1.0f / fE;
+ afU[i1] = (afR[i1] - fA * afU[i0]) * fInvE;
+ }
+ for (i0 = iSize - 1, i1 = iSize - 2; i1 >= 0; i0--, i1--)
+ afU[i1] -= afD[i1] * afU[i0];
+
+ delete[] afD;
+ return true;
+}
+//----------------------------------------------------------------------------
+bool LinearSystem::SolveSymmetric(int iSize, Real **aafA, Real *afB)
+{
+ // A = L D L^t decomposition with diagonal terms of L equal to 1. The
+ // algorithm stores D terms in A[i][i] and off-diagonal L terms in
+ // A[i][j] for i > j. (G. Golub and C. Van Loan, Matrix Computations)
+
+ const Real fTolerance = 1e-06f;
+
+ int i0, i1;
+ Real *afV = new Real[iSize];
+ assert(afV);
+
+ for (i1 = 0; i1 < iSize; i1++)
+ {
+ for (i0 = 0; i0 < i1; i0++)
+ afV[i0] = aafA[i1][i0] * aafA[i0][i0];
+
+ afV[i1] = aafA[i1][i1];
+ for (i0 = 0; i0 < i1; i0++)
+ afV[i1] -= aafA[i1][i0] * afV[i0];
+
+ aafA[i1][i1] = afV[i1];
+ if (Math::FAbs(afV[i1]) <= fTolerance)
+ {
+ delete[] afV;
+ return false;
+ }
+
+ Real fInv = 1.0f / afV[i1];
+ for (i0 = i1 + 1; i0 < iSize; i0++)
+ {
+ for (int i2 = 0; i2 < i1; i2++)
+ aafA[i0][i1] -= aafA[i0][i2] * afV[i2];
+ aafA[i0][i1] *= fInv;
+ }
+ }
+ delete[] afV;
+
+ // Solve Ax = B.
+
+ // Forward substitution: Let z = DL^t x, then Lz = B. Algorithm
+ // stores z terms in B vector.
+ for (i0 = 0; i0 < iSize; i0++)
+ {
+ for (i1 = 0; i1 < i0; i1++)
+ afB[i0] -= aafA[i0][i1] * afB[i1];
+ }
+
+ // Diagonal division: Let y = L^t x, then Dy = z. Algorithm stores
+ // y terms in B vector.
+ for (i0 = 0; i0 < iSize; i0++)
+ {
+ if (Math::FAbs(aafA[i0][i0]) <= fTolerance)
+ return false;
+ afB[i0] /= aafA[i0][i0];
+ }
+
+ // Back substitution: Solve L^t x = y. Algorithm stores x terms in
+ // B vector.
+ for (i0 = iSize - 2; i0 >= 0; i0--)
+ {
+ for (i1 = i0 + 1; i1 < iSize; i1++)
+ afB[i0] -= aafA[i1][i0] * afB[i1];
+ }
+
+ return true;
+}
+//----------------------------------------------------------------------------
+bool LinearSystem::SymmetricInverse(int iSize, Real **aafA,
+ Real **aafAInv)
+{
+ // Same algorithm as SolveSymmetric, but applied simultaneously to
+ // columns of identity matrix.
+
+ Real *afV = new Real[iSize];
+ assert(afV);
+
+ int i0, i1;
+ for (i0 = 0; i0 < iSize; i0++)
+ {
+ for (i1 = 0; i1 < iSize; i1++)
+ aafAInv[i0][i1] = (i0 != i1 ? 0.0f : 1.0f);
+ }
+
+ for (i1 = 0; i1 < iSize; i1++)
+ {
+ for (i0 = 0; i0 < i1; i0++)
+ afV[i0] = aafA[i1][i0] * aafA[i0][i0];
+
+ afV[i1] = aafA[i1][i1];
+ for (i0 = 0; i0 < i1; i0++)
+ afV[i1] -= aafA[i1][i0] * afV[i0];
+
+ aafA[i1][i1] = afV[i1];
+ for (i0 = i1 + 1; i0 < iSize; i0++)
+ {
+ for (int i2 = 0; i2 < i1; i2++)
+ aafA[i0][i1] -= aafA[i0][i2] * afV[i2];
+ aafA[i0][i1] /= afV[i1];
+ }
+ }
+ delete[] afV;
+
+ for (int iCol = 0; iCol < iSize; iCol++)
+ {
+ // forward substitution
+ for (i0 = 0; i0 < iSize; i0++)
+ {
+ for (i1 = 0; i1 < i0; i1++)
+ aafAInv[i0][iCol] -= aafA[i0][i1] * aafAInv[i1][iCol];
+ }
+
+ // diagonal division
+ const Real fTolerance = 1e-06f;
+ for (i0 = 0; i0 < iSize; i0++)
+ {
+ if (fabs(aafA[i0][i0]) <= fTolerance)
+ return false;
+ aafAInv[i0][iCol] /= aafA[i0][i0];
+ }
+
+ // back substitution
+ for (i0 = iSize - 2; i0 >= 0; i0--)
+ {
+ for (i1 = i0 + 1; i1 < iSize; i1++)
+ aafAInv[i0][iCol] -= aafA[i1][i0] * aafAInv[i1][iCol];
+ }
+ }
+
+ return true;
+}
+//----------------------------------------------------------------------------
diff --git a/src/editors/FreeMagic/MgcLinearSystem.h b/src/editors/FreeMagic/MgcLinearSystem.h
new file mode 100644
index 00000000000..b3195687514
--- /dev/null
+++ b/src/editors/FreeMagic/MgcLinearSystem.h
@@ -0,0 +1,104 @@
+// Magic Software, Inc.
+// http://www.magic-software.com
+// Copyright (c) 2000-2002. All Rights Reserved
+//
+// Source code from Magic Software is supplied under the terms of a license
+// agreement and may not be copied or disclosed except in accordance with the
+// terms of that agreement. The various license agreements may be found at
+// the Magic Software web site. This file is subject to the license
+//
+// FREE SOURCE CODE
+// http://www.magic-software.com/License/free.pdf
+
+#ifndef MGCLINEARSYSTEM_H
+#define MGCLINEARSYSTEM_H
+
+#include "MgcMath.h"
+
+namespace Mgc
+{
+
+ class MAGICFM LinearSystem
+ {
+ public:
+ // 2x2 and 3x3 systems (avoids overhead of Gaussian elimination)
+ static Real &Tolerance();
+ static bool Solve2(const Real aafA[2][2], const Real afB[2],
+ Real afX[2]);
+ static bool Solve3(const Real aafA[3][3], const Real afB[3],
+ Real afX[3]);
+
+ // convenience for allocation, memory is zeroed out
+ static Real **NewMatrix(int iSize);
+ static void DeleteMatrix(int iSize, Real **aafA);
+ static Real *NewVector(int iSize);
+ static void DeleteVector(Real *afB);
+
+ // Input:
+ // A[iSize][iSize], entries are A[row][col]
+ // Output:
+ // return value is TRUE if successful, FALSE if pivoting failed
+ // A[iSize][iSize], inverse matrix
+ static bool Inverse(int iSize, Real **aafA);
+
+ // Input:
+ // A[iSize][iSize] coefficient matrix, entries are A[row][col]
+ // B[iSize] vector, entries are B[row]
+ // Output:
+ // return value is TRUE if successful, FALSE if pivoting failed
+ // A[iSize][iSize] is inverse matrix
+ // B[iSize] is solution x to Ax = B
+ static bool Solve(int iSize, Real **aafA, Real *afB);
+
+ // Input:
+ // Matrix is tridiagonal.
+ // Lower diagonal A[iSize-1]
+ // Main diagonal B[iSize]
+ // Upper diagonal C[iSize-1]
+ // Right-hand side R[iSize]
+ // Output:
+ // return value is TRUE if successful, FALSE if pivoting failed
+ // U[iSize] is solution
+ static bool SolveTri(int iSize, Real *afA, Real *afB, Real *afC,
+ Real *afR, Real *afU);
+
+ // Input:
+ // Matrix is tridiagonal.
+ // Lower diagonal is constant, A
+ // Main diagonal is constant, B
+ // Upper diagonal is constant, C
+ // Right-hand side Rr[iSize]
+ // Output:
+ // return value is TRUE if successful, FALSE if pivoting failed
+ // U[iSize] is solution
+ static bool SolveConstTri(int iSize, Real fA, Real fB, Real fC,
+ Real *afR, Real *afU);
+
+ // Input:
+ // A[iSize][iSize] symmetric matrix, entries are A[row][col]
+ // B[iSize] vector, entries are B[row]
+ // Output:
+ // return value is TRUE if successful, FALSE if (nearly) singular
+ // decomposition A = L D L^t (diagonal terms of L are all 1)
+ // A[i][i] = entries of diagonal D
+ // A[i][j] for i > j = lower triangular part of L
+ // B[iSize] is solution to x to Ax = B
+ static bool SolveSymmetric(int iSize, Real **aafA, Real *afB);
+
+ // Input:
+ // A[iSize][iSize], entries are A[row][col]
+ // Output:
+ // return value is TRUE if successful, FALSE if algorithm failed
+ // AInv[iSize][iSize], inverse matrix
+ static bool SymmetricInverse(int iSize, Real **aafA, Real **aafAInv);
+
+ protected:
+ // tolerance for 2x2 and 3x3 system solving
+ static Real ms_fTolerance;
+ };
+
+#include "MgcLinearSystem.inl"
+
+} // namespace Mgc
+
+#endif
diff --git a/src/editors/FreeMagic/MgcLinearSystem.inl b/src/editors/FreeMagic/MgcLinearSystem.inl
new file mode 100644
index 00000000000..31ba62cb619
--- /dev/null
+++ b/src/editors/FreeMagic/MgcLinearSystem.inl
@@ -0,0 +1,18 @@
+// Magic Software, Inc.
+// http://www.magic-software.com
+// Copyright (c) 2000-2002. All Rights Reserved
+//
+// Source code from Magic Software is supplied under the terms of a license
+// agreement and may not be copied or disclosed except in accordance with the
+// terms of that agreement. The various license agreements may be found at
+// the Magic Software web site. This file is subject to the license
+//
+// FREE SOURCE CODE
+// http://www.magic-software.com/License/free.pdf
+
+//----------------------------------------------------------------------------
+inline Real &LinearSystem::Tolerance()
+{
+ return ms_fTolerance;
+}
+//----------------------------------------------------------------------------
diff --git a/src/editors/FreeMagic/MgcMath.cpp b/src/editors/FreeMagic/MgcMath.cpp
new file mode 100644
index 00000000000..88edab218ed
--- /dev/null
+++ b/src/editors/FreeMagic/MgcMath.cpp
@@ -0,0 +1,213 @@
+// Magic Software, Inc.
+// http://www.magic-software.com
+// Copyright (c) 2000-2002. All Rights Reserved
+//
+// Source code from Magic Software is supplied under the terms of a license
+// agreement and may not be copied or disclosed except in accordance with the
+// terms of that agreement. The various license agreements may be found at
+// the Magic Software web site. This file is subject to the license
+//
+// FREE SOURCE CODE
+// http://www.magic-software.com/License/free.pdf
+//#include "stdafx.h"
+#pragma hdrstop
+
+#include "MgcMath.h"
+using namespace Mgc;
+
+#ifdef MGC_USE_DOUBLE
+const Real Mgc::Math::MAX_REAL = DBL_MAX;
+#else
+const Real Mgc::Math::MAX_REAL = FLT_MAX;
+#endif
+
+const Real Mgc::Math::_PI = 4.0f * Mgc::Math::ATan(1.0f);
+const Real Mgc::Math::TWO_PI = 2.0f * _PI;
+const Real Mgc::Math::HALF_PI = 0.5f * _PI;
+const Real Mgc::Math::INV_TWO_PI = 1.0f / TWO_PI;
+const Real Mgc::Math::DEG_TO_RAD = Mgc::Math::_PI / 180.0f;
+const Real Mgc::Math::RAD_TO_DEG = 1.0f / Mgc::Math::DEG_TO_RAD;
+
+//----------------------------------------------------------------------------
+Real Mgc::Math::UnitRandom(Real fSeed)
+{
+ if (fSeed > 0.0f)
+ srand((unsigned int)fSeed);
+
+ return Real(rand()) / Real(RAND_MAX);
+}
+//----------------------------------------------------------------------------
+Real Mgc::Math::SymmetricRandom(Real fSeed)
+{
+ if (fSeed > 0.0f)
+ srand((unsigned int)fSeed);
+
+ return 2.0f * Real(rand()) / Real(RAND_MAX) - 1.0f;
+}
+//----------------------------------------------------------------------------
+Real Mgc::Math::IntervalRandom(Real fMin, Real fMax, Real fSeed)
+{
+ if (fSeed > 0.0f)
+ srand((unsigned int)fSeed);
+
+ return fMin + (fMax - fMin) * Real(rand()) / Real(RAND_MAX);
+}
+//----------------------------------------------------------------------------
+Real Mgc::Math::FastSin0(Real fAngle)
+{
+ Real fASqr = fAngle * fAngle;
+ Real fResult = 7.61e-03f;
+ fResult *= fASqr;
+ fResult -= 1.6605e-01f;
+ fResult *= fASqr;
+ fResult += 1.0f;
+ fResult *= fAngle;
+ return fResult;
+}
+//----------------------------------------------------------------------------
+Real Mgc::Math::FastSin1(Real fAngle)
+{
+ Real fASqr = fAngle * fAngle;
+ Real fResult = -2.39e-08f;
+ fResult *= fASqr;
+ fResult += 2.7526e-06f;
+ fResult *= fASqr;
+ fResult -= 1.98409e-04f;
+ fResult *= fASqr;
+ fResult += 8.3333315e-03f;
+ fResult *= fASqr;
+ fResult -= 1.666666664e-01f;
+ fResult *= fASqr;
+ fResult += 1.0f;
+ fResult *= fAngle;
+ return fResult;
+}
+//----------------------------------------------------------------------------
+Real Mgc::Math::FastCos0(Real fAngle)
+{
+ Real fASqr = fAngle * fAngle;
+ Real fResult = 3.705e-02f;
+ fResult *= fASqr;
+ fResult -= 4.967e-01f;
+ fResult *= fASqr;
+ fResult += 1.0f;
+ return fResult;
+}
+//----------------------------------------------------------------------------
+Real Mgc::Math::FastCos1(Real fAngle)
+{
+ Real fASqr = fAngle * fAngle;
+ Real fResult = -2.605e-07f;
+ fResult *= fASqr;
+ fResult += 2.47609e-05f;
+ fResult *= fASqr;
+ fResult -= 1.3888397e-03f;
+ fResult *= fASqr;
+ fResult += 4.16666418e-02f;
+ fResult *= fASqr;
+ fResult -= 4.999999963e-01f;
+ fResult *= fASqr;
+ fResult += 1.0f;
+ return fResult;
+}
+//----------------------------------------------------------------------------
+Real Mgc::Math::FastTan0(Real fAngle)
+{
+ Real fASqr = fAngle * fAngle;
+ Real fResult = 2.033e-01f;
+ fResult *= fASqr;
+ fResult += 3.1755e-01f;
+ fResult *= fASqr;
+ fResult += 1.0f;
+ fResult *= fAngle;
+ return fResult;
+}
+//----------------------------------------------------------------------------
+Real Mgc::Math::FastTan1(Real fAngle)
+{
+ Real fASqr = fAngle * fAngle;
+ Real fResult = 9.5168091e-03f;
+ fResult *= fASqr;
+ fResult += 2.900525e-03f;
+ fResult *= fASqr;
+ fResult += 2.45650893e-02f;
+ fResult *= fASqr;
+ fResult += 5.33740603e-02f;
+ fResult *= fASqr;
+ fResult += 1.333923995e-01f;
+ fResult *= fASqr;
+ fResult += 3.333314036e-01f;
+ fResult *= fASqr;
+ fResult += 1.0f;
+ fResult *= fAngle;
+ return fResult;
+}
+//----------------------------------------------------------------------------
+Real Mgc::Math::FastInvSin(Real fValue)
+{
+ Real fRoot = Mgc::Math::Sqrt(1.0f - fValue);
+ Real fResult = -0.0187293f;
+ fResult *= fValue;
+ fResult += 0.0742610f;
+ fResult *= fValue;
+ fResult -= 0.2121144f;
+ fResult *= fValue;
+ fResult += 1.5707288f;
+ fResult = HALF_PI - fRoot * fResult;
+ return fResult;
+}
+//----------------------------------------------------------------------------
+Real Mgc::Math::FastInvCos(Real fValue)
+{
+ Real fRoot = Mgc::Math::Sqrt(1.0f - fValue);
+ Real fResult = -0.0187293f;
+ fResult *= fValue;
+ fResult += 0.0742610f;
+ fResult *= fValue;
+ fResult -= 0.2121144f;
+ fResult *= fValue;
+ fResult += 1.5707288f;
+ fResult *= fRoot;
+ return fResult;
+}
+//----------------------------------------------------------------------------
+Real Mgc::Math::FastInvTan0(Real fValue)
+{
+ Real fVSqr = fValue * fValue;
+ Real fResult = 0.0208351f;
+ fResult *= fVSqr;
+ fResult -= 0.085133f;
+ fResult *= fVSqr;
+ fResult += 0.180141f;
+ fResult *= fVSqr;
+ fResult -= 0.3302995f;
+ fResult *= fVSqr;
+ fResult += 0.999866f;
+ fResult *= fValue;
+ return fResult;
+}
+//----------------------------------------------------------------------------
+Real Mgc::Math::FastInvTan1(Real fValue)
+{
+ Real fVSqr = fValue * fValue;
+ Real fResult = 0.0028662257f;
+ fResult *= fVSqr;
+ fResult -= 0.0161657367f;
+ fResult *= fVSqr;
+ fResult += 0.0429096138f;
+ fResult *= fVSqr;
+ fResult -= 0.0752896400f;
+ fResult *= fVSqr;
+ fResult += 0.1065626393f;
+ fResult *= fVSqr;
+ fResult -= 0.1420889944f;
+ fResult *= fVSqr;
+ fResult += 0.1999355085f;
+ fResult *= fVSqr;
+ fResult -= 0.3333314528f;
+ fResult *= fVSqr;
+ fResult += 1.0f;
+ fResult *= fValue;
+ return fResult;
+}
+//----------------------------------------------------------------------------
diff --git a/src/editors/FreeMagic/MgcMath.h b/src/editors/FreeMagic/MgcMath.h
new file mode 100644
index 00000000000..dc2836e07f2
--- /dev/null
+++ b/src/editors/FreeMagic/MgcMath.h
@@ -0,0 +1,126 @@
+// Magic Software, Inc.
+// http://www.magic-software.com
+// Copyright (c) 2000-2002. All Rights Reserved
+//
+// Source code from Magic Software is supplied under the terms of a license
+// agreement and may not be copied or disclosed except in accordance with the
+// terms of that agreement. The various license agreements may be found at
+// the Magic Software web site. This file is subject to the license
+//
+// FREE SOURCE CODE
+// http://www.magic-software.com/License/free.pdf
+
+#ifndef MGCMATH_H
+#define MGCMATH_H
+
+#include "MagicFMLibType.h"
+#include "MgcRTLib.h"
+
+namespace Mgc
+{
+
+#ifdef MGC_USE_DOUBLE
+ typedef double Real;
+#else
+ typedef float Real;
+#endif
+
+ class MAGICFM Math
+ {
+ public:
+ // Return -1 if the input is negative, 0 if the input is zero, and +1
+ // if the input is positive.
+ static int Sign(int iValue);
+ static Real Sign(Real fValue);
+
+ // Just computes fValue*fValue.
+ static Real Sqr(Real fValue);
+
+ // Generate a random number in [0,1). The random number generator may
+ // be seeded by a first call to UnitRandom with a positive seed.
+ static Real UnitRandom(Real fSeed = 0.0f);
+
+ // Generate a random number in [-1,1). The random number generator may
+ // be seeded by a first call to SymmetricRandom with a positive seed.
+ static Real SymmetricRandom(Real fSeed = 0.0f);
+
+ // Generate a random number in [min,max). The random number generator may
+ // be seeded by a first call to IntervalRandom with a positive seed.
+ static Real IntervalRandom(Real fMin, Real fMax, Real fSeed = 0.0f);
+
+ // Fast evaluation of sin(angle) by polynomial approximations. The angle
+ // must be in [0,pi/2]. The maximum absolute error is about 1.7e-04 for
+ // FastSin0 and about 2.3e-09 for FastSin1. The speed up is about 2 for
+ // FastSin0 and about 1.5 for FastSin1.
+ static Real FastSin0(Real fAngle);
+ static Real FastSin1(Real fAngle);
+
+ // Fast evaluation of cos(angle) by polynomial approximations. The angle
+ // must be in [0,pi/2]. The maximum absolute error is about 1.2e-03 for
+ // FastCos0 and about 2.3e-09 for FastCos1. The speed up is about 2 for
+ // FastCos0 and about 1.5 for FastCos1.
+ static Real FastCos0(Real fAngle);
+ static Real FastCos1(Real fAngle);
+
+ // Fast evaluation of tan(angle) by polynomial approximations. The angle
+ // must be in [0,pi/4]. The maximum absolute error is about 8.1e-04 for
+ // FastTan0 and about 1.9e-08 for FastTan1. The speed up is about 2.5 for
+ // FastTan0 and about 1.75 for FastTan1.
+ static Real FastTan0(Real fAngle);
+ static Real FastTan1(Real fAngle);
+
+ // Fast evaluation of asin(value) by a sqrt times a polynomial. The value
+ // must be in [0,1]. The maximum absolute error is about 6.8e-05 and the
+ // speed up is about 2.5.
+ static Real FastInvSin(Real fValue);
+
+ // Fast evaluation of acos(value) by a sqrt times a polynomial. The value
+ // must be in [0,1]. The maximum absolute error is about 6.8e-05 and the
+ // speed up is about 2.5.
+ static Real FastInvCos(Real fValue);
+
+ // Fast evaluation of atan(value) by polynomial approximations. The value
+ // must be in [-1,1]. The maximum absolute error is about 1.2e-05 for
+ // FastInvTan0 and about 1.43-08 for FastInvTan1. The speed up is about
+ // 2.2 for FastInvTan0 and about 1.5 for FastInvTan1.
+ static Real FastInvTan0(Real fValue);
+ static Real FastInvTan1(Real fValue);
+
+ // Wrappers for acos and asin, but the input value is clamped to [-1,1]
+ // to avoid silent returns of NaN.
+ static Real ACos(Real fValue);
+ static Real ASin(Real fValue);
+
+ // Wrapper for 1/sqrt to allow a fast implementation to replace it at a
+ // later date.
+ static Real InvSqrt(Real fValue);
+
+ // Wrappers to hide 'double foo(double)' versus 'float foof(float)'.
+ static Real ATan(Real fValue);
+ static Real ATan2(Real fY, Real fX);
+ static Real Ceil(Real fValue);
+ static Real Cos(Real fValue);
+ static Real Exp(Real fValue);
+ static Real FAbs(Real fValue);
+ static Real Floor(Real fValue);
+ static Real Log(Real fValue);
+ static Real Pow(Real kBase, Real kExponent);
+ static Real Sin(Real fValue);
+ static Real Sqrt(Real fValue);
+ static Real Tan(Real fValue);
+
+ // common constants
+ static const Real MAX_REAL;
+ static const Real _PI;
+ static const Real TWO_PI;
+ static const Real HALF_PI;
+ static const Real INV_TWO_PI;
+ static const Real DEG_TO_RAD;
+ static const Real RAD_TO_DEG;
+ };
+
+#include "MgcMath.inl"
+
+} // namespace Mgc
+
+#endif
diff --git a/src/editors/FreeMagic/MgcMath.inl b/src/editors/FreeMagic/MgcMath.inl
new file mode 100644
index 00000000000..3a30685f83e
--- /dev/null
+++ b/src/editors/FreeMagic/MgcMath.inl
@@ -0,0 +1,140 @@
+// Magic Software, Inc.
+// http://www.magic-software.com
+// Copyright (c) 2000-2002. All Rights Reserved
+//
+// Source code from Magic Software is supplied under the terms of a license
+// agreement and may not be copied or disclosed except in accordance with the
+// terms of that agreement. The various license agreements may be found at
+// the Magic Software web site. This file is subject to the license
+//
+// FREE SOURCE CODE
+// http://www.magic-software.com/License/free.pdf
+#include
+//----------------------------------------------------------------------------
+
+#ifndef M_PI
+#define M_PI 3.14159265358979323846 /* pi */
+#endif
+
+inline Real Math::ACos(Real fValue)
+{
+ if (-1.0f < fValue)
+ {
+ if (fValue < 1.0f)
+ return (Real)acos(fValue);
+ else
+ return 0.0f;
+ }
+ else
+ {
+ return M_PI;
+ }
+}
+//----------------------------------------------------------------------------
+inline Real Math::ASin(Real fValue)
+{
+ if (-1.0f < fValue)
+ {
+ if (fValue < 1.0f)
+ return (Real)asin(fValue);
+ else
+ return -HALF_PI;
+ }
+ else
+ {
+ return HALF_PI;
+ }
+}
+//----------------------------------------------------------------------------
+inline Real Math::ATan(Real fValue)
+{
+ return (Real)atan(fValue);
+}
+//----------------------------------------------------------------------------
+inline Real Math::ATan2(Real fY, Real fX)
+{
+ return (Real)atan2(fY, fX);
+}
+//----------------------------------------------------------------------------
+inline Real Math::Ceil(Real fValue)
+{
+ return (Real)ceil(fValue);
+}
+//----------------------------------------------------------------------------
+inline Real Math::Cos(Real fValue)
+{
+ return (Real)cos(fValue);
+}
+//----------------------------------------------------------------------------
+inline Real Math::Exp(Real fValue)
+{
+ return (Real)exp(fValue);
+}
+//----------------------------------------------------------------------------
+inline Real Math::FAbs(Real fValue)
+{
+ return (Real)fabs(fValue);
+}
+//----------------------------------------------------------------------------
+inline Real Math::Floor(Real fValue)
+{
+ return (Real)floor(fValue);
+}
+//----------------------------------------------------------------------------
+inline Real Math::InvSqrt(Real fValue)
+{
+ return (Real)(1.0 / sqrt(fValue));
+}
+//----------------------------------------------------------------------------
+inline Real Math::Log(Real fValue)
+{
+ return (Real)log(fValue);
+}
+//----------------------------------------------------------------------------
+inline Real Math::Pow(Real kBase, Real kExponent)
+{
+ return (Real)pow(kBase, kExponent);
+}
+//----------------------------------------------------------------------------
+inline int Math::Sign(int iValue)
+{
+ if (iValue > 0)
+ return 1;
+
+ if (iValue < 0)
+ return -1;
+
+ return 0;
+}
+//----------------------------------------------------------------------------
+inline Real Math::Sign(Real fValue)
+{
+ if (fValue > 0.0f)
+ return 1.0f;
+
+ if (fValue < 0.0f)
+ return -1.0f;
+
+ return 0.0f;
+}
+//----------------------------------------------------------------------------
+inline Real Math::Sin(Real fValue)
+{
+ return (Real)sin(fValue);
+}
+//----------------------------------------------------------------------------
+inline Real Math::Sqr(Real fValue)
+{
+ return fValue * fValue;
+}
+//----------------------------------------------------------------------------
+inline Real Math::Sqrt(Real fValue)
+{
+ return (Real)sqrt(fValue);
+}
+//----------------------------------------------------------------------------
+inline Real Math::Tan(Real fValue)
+{
+ return (Real)tan(fValue);
+}
+//----------------------------------------------------------------------------
diff --git a/src/editors/FreeMagic/MgcMatrix3.cpp b/src/editors/FreeMagic/MgcMatrix3.cpp
new file mode 100644
index 00000000000..3182c847fa4
--- /dev/null
+++ b/src/editors/FreeMagic/MgcMatrix3.cpp
@@ -0,0 +1,1558 @@
+// Magic Software, Inc.
+// http://www.magic-software.com
+// Copyright (c) 2000-2002. All Rights Reserved
+//
+// Source code from Magic Software is supplied under the terms of a license
+// agreement and may not be copied or disclosed except in accordance with the
+// terms of that agreement. The various license agreements may be found at
+// the Magic Software web site. This file is subject to the license
+//
+// FREE SOURCE CODE
+// http://www.magic-software.com/License/free.pdf
+
+#include "MgcMatrix3.h"
+using namespace Mgc;
+
+const Real Matrix3::EPSILON = 1e-06f;
+const Matrix3 Matrix3::ZERO(0.0f, 0.0f, 0.0f, 0.0f, 0.0f, 0.0f, 0.0f, 0.0f, 0.0f);
+const Matrix3 Matrix3::IDENTITY(1.0f, 0.0f, 0.0f, 0.0f, 1.0f, 0.0f, 0.0f, 0.0f, 1.0f);
+const Real Matrix3::ms_fSvdEpsilon = 1e-04f;
+const int Matrix3::ms_iSvdMaxIterations = 32;
+
+//----------------------------------------------------------------------------
+Matrix3::Matrix3(const Real aafEntry[3][3])
+{
+ memcpy(m_aafEntry, aafEntry, 9 * sizeof(Real));
+}
+//----------------------------------------------------------------------------
+Matrix3::Matrix3(const Matrix3 &rkMatrix)
+{
+ memcpy(m_aafEntry, rkMatrix.m_aafEntry, 9 * sizeof(Real));
+}
+//----------------------------------------------------------------------------
+Matrix3::Matrix3(Real fEntry00, Real fEntry01, Real fEntry02,
+ Real fEntry10, Real fEntry11, Real fEntry12, Real fEntry20,
+ Real fEntry21, Real fEntry22)
+{
+ m_aafEntry[0][0] = fEntry00;
+ m_aafEntry[0][1] = fEntry01;
+ m_aafEntry[0][2] = fEntry02;
+ m_aafEntry[1][0] = fEntry10;
+ m_aafEntry[1][1] = fEntry11;
+ m_aafEntry[1][2] = fEntry12;
+ m_aafEntry[2][0] = fEntry20;
+ m_aafEntry[2][1] = fEntry21;
+ m_aafEntry[2][2] = fEntry22;
+}
+//----------------------------------------------------------------------------
+Vector3 Matrix3::GetColumn(int iCol) const
+{
+ assert(0 <= iCol && iCol < 3);
+ return Vector3(m_aafEntry[0][iCol], m_aafEntry[1][iCol],
+ m_aafEntry[2][iCol]);
+}
+//----------------------------------------------------------------------------
+Matrix3 &Matrix3::operator=(const Matrix3 &rkMatrix)
+{
+ memcpy(m_aafEntry, rkMatrix.m_aafEntry, 9 * sizeof(Real));
+ return *this;
+}
+//----------------------------------------------------------------------------
+bool Matrix3::operator==(const Matrix3 &rkMatrix) const
+{
+ for (int iRow = 0; iRow < 3; iRow++)
+ {
+ for (int iCol = 0; iCol < 3; iCol++)
+ {
+ if (m_aafEntry[iRow][iCol] != rkMatrix.m_aafEntry[iRow][iCol])
+ return false;
+ }
+ }
+
+ return true;
+}
+//----------------------------------------------------------------------------
+bool Matrix3::operator!=(const Matrix3 &rkMatrix) const
+{
+ return !operator==(rkMatrix);
+}
+//----------------------------------------------------------------------------
+Matrix3 Matrix3::operator+(const Matrix3 &rkMatrix) const
+{
+ Matrix3 kSum;
+ for (int iRow = 0; iRow < 3; iRow++)
+ {
+ for (int iCol = 0; iCol < 3; iCol++)
+ {
+ kSum.m_aafEntry[iRow][iCol] = m_aafEntry[iRow][iCol] +
+ rkMatrix.m_aafEntry[iRow][iCol];
+ }
+ }
+ return kSum;
+}
+//----------------------------------------------------------------------------
+Matrix3 Matrix3::operator-(const Matrix3 &rkMatrix) const
+{
+ Matrix3 kDiff;
+ for (int iRow = 0; iRow < 3; iRow++)
+ {
+ for (int iCol = 0; iCol < 3; iCol++)
+ {
+ kDiff.m_aafEntry[iRow][iCol] = m_aafEntry[iRow][iCol] -
+ rkMatrix.m_aafEntry[iRow][iCol];
+ }
+ }
+ return kDiff;
+}
+//----------------------------------------------------------------------------
+Matrix3 Matrix3::operator*(const Matrix3 &rkMatrix) const
+{
+ Matrix3 kProd;
+ for (int iRow = 0; iRow < 3; iRow++)
+ {
+ for (int iCol = 0; iCol < 3; iCol++)
+ {
+ kProd.m_aafEntry[iRow][iCol] =
+ m_aafEntry[iRow][0] * rkMatrix.m_aafEntry[0][iCol] +
+ m_aafEntry[iRow][1] * rkMatrix.m_aafEntry[1][iCol] +
+ m_aafEntry[iRow][2] * rkMatrix.m_aafEntry[2][iCol];
+ }
+ }
+ return kProd;
+}
+//----------------------------------------------------------------------------
+Vector3 Matrix3::operator*(const Vector3 &rkPoint) const
+{
+ Vector3 kProd;
+ for (int iRow = 0; iRow < 3; iRow++)
+ {
+ kProd[iRow] =
+ m_aafEntry[iRow][0] * rkPoint[0] +
+ m_aafEntry[iRow][1] * rkPoint[1] +
+ m_aafEntry[iRow][2] * rkPoint[2];
+ }
+ return kProd;
+}
+//----------------------------------------------------------------------------
+Vector3 Mgc::operator*(const Vector3 &rkPoint, const Matrix3 &rkMatrix)
+{
+ Vector3 kProd;
+ for (int iRow = 0; iRow < 3; iRow++)
+ {
+ kProd[iRow] =
+ rkPoint[0] * rkMatrix.m_aafEntry[0][iRow] +
+ rkPoint[1] * rkMatrix.m_aafEntry[1][iRow] +
+ rkPoint[2] * rkMatrix.m_aafEntry[2][iRow];
+ }
+ return kProd;
+}
+//----------------------------------------------------------------------------
+Matrix3 Matrix3::operator-() const
+{
+ Matrix3 kNeg;
+ for (int iRow = 0; iRow < 3; iRow++)
+ {
+ for (int iCol = 0; iCol < 3; iCol++)
+ kNeg[iRow][iCol] = -m_aafEntry[iRow][iCol];
+ }
+ return kNeg;
+}
+//----------------------------------------------------------------------------
+Matrix3 Matrix3::operator*(Real fScalar) const
+{
+ Matrix3 kProd;
+ for (int iRow = 0; iRow < 3; iRow++)
+ {
+ for (int iCol = 0; iCol < 3; iCol++)
+ kProd[iRow][iCol] = fScalar * m_aafEntry[iRow][iCol];
+ }
+ return kProd;
+}
+//----------------------------------------------------------------------------
+Matrix3 Mgc::operator*(Real fScalar, const Matrix3 &rkMatrix)
+{
+ Matrix3 kProd;
+ for (int iRow = 0; iRow < 3; iRow++)
+ {
+ for (int iCol = 0; iCol < 3; iCol++)
+ kProd[iRow][iCol] = fScalar * rkMatrix.m_aafEntry[iRow][iCol];
+ }
+ return kProd;
+}
+//----------------------------------------------------------------------------
+Matrix3 Matrix3::TransposeTimes(const Matrix3 &rkM) const
+{
+ Matrix3 kProd;
+ for (int iRow = 0; iRow < 3; iRow++)
+ {
+ for (int iCol = 0; iCol < 3; iCol++)
+ {
+ kProd[iRow][iCol] = 0.0f;
+ for (int iMid = 0; iMid < 3; iMid++)
+ {
+ kProd[iRow][iCol] += m_aafEntry[iMid][iRow] *
+ rkM.m_aafEntry[iMid][iCol];
+ }
+ }
+ }
+ return kProd;
+}
+//----------------------------------------------------------------------------
+Matrix3 Matrix3::TimesTranspose(const Matrix3 &rkM) const
+{
+ Matrix3 kProd;
+ for (int iRow = 0; iRow < 3; iRow++)
+ {
+ for (int iCol = 0; iCol < 3; iCol++)
+ {
+ kProd[iRow][iCol] = 0.0f;
+ for (int iMid = 0; iMid < 3; iMid++)
+ {
+ kProd[iRow][iCol] += m_aafEntry[iRow][iMid] *
+ rkM.m_aafEntry[iCol][iMid];
+ }
+ }
+ }
+ return kProd;
+}
+//----------------------------------------------------------------------------
+Matrix3 Matrix3::Transpose() const
+{
+ Matrix3 kTranspose;
+ for (int iRow = 0; iRow < 3; iRow++)
+ {
+ for (int iCol = 0; iCol < 3; iCol++)
+ kTranspose[iRow][iCol] = m_aafEntry[iCol][iRow];
+ }
+ return kTranspose;
+}
+//----------------------------------------------------------------------------
+bool Matrix3::Inverse(Matrix3 &rkInverse, Real fTolerance) const
+{
+ // Invert a 3x3 using cofactors. This is about 8 times faster than
+ // the Numerical Recipes code which uses Gaussian elimination.
+
+ rkInverse[0][0] = m_aafEntry[1][1] * m_aafEntry[2][2] -
+ m_aafEntry[1][2] * m_aafEntry[2][1];
+ rkInverse[0][1] = m_aafEntry[0][2] * m_aafEntry[2][1] -
+ m_aafEntry[0][1] * m_aafEntry[2][2];
+ rkInverse[0][2] = m_aafEntry[0][1] * m_aafEntry[1][2] -
+ m_aafEntry[0][2] * m_aafEntry[1][1];
+ rkInverse[1][0] = m_aafEntry[1][2] * m_aafEntry[2][0] -
+ m_aafEntry[1][0] * m_aafEntry[2][2];
+ rkInverse[1][1] = m_aafEntry[0][0] * m_aafEntry[2][2] -
+ m_aafEntry[0][2] * m_aafEntry[2][0];
+ rkInverse[1][2] = m_aafEntry[0][2] * m_aafEntry[1][0] -
+ m_aafEntry[0][0] * m_aafEntry[1][2];
+ rkInverse[2][0] = m_aafEntry[1][0] * m_aafEntry[2][1] -
+ m_aafEntry[1][1] * m_aafEntry[2][0];
+ rkInverse[2][1] = m_aafEntry[0][1] * m_aafEntry[2][0] -
+ m_aafEntry[0][0] * m_aafEntry[2][1];
+ rkInverse[2][2] = m_aafEntry[0][0] * m_aafEntry[1][1] -
+ m_aafEntry[0][1] * m_aafEntry[1][0];
+
+ Real fDet =
+ m_aafEntry[0][0] * rkInverse[0][0] +
+ m_aafEntry[0][1] * rkInverse[1][0] +
+ m_aafEntry[0][2] * rkInverse[2][0];
+
+ if (Math::FAbs(fDet) <= fTolerance)
+ return false;
+
+ Real fInvDet = 1.0f / fDet;
+ for (int iRow = 0; iRow < 3; iRow++)
+ {
+ for (int iCol = 0; iCol < 3; iCol++)
+ rkInverse[iRow][iCol] *= fInvDet;
+ }
+
+ return true;
+}
+//----------------------------------------------------------------------------
+Matrix3 Matrix3::Inverse(Real fTolerance) const
+{
+ Matrix3 kInverse = Matrix3::ZERO;
+ Inverse(kInverse, fTolerance);
+ return kInverse;
+}
+//----------------------------------------------------------------------------
+Real Matrix3::Determinant() const
+{
+ Real fCofactor00 = m_aafEntry[1][1] * m_aafEntry[2][2] -
+ m_aafEntry[1][2] * m_aafEntry[2][1];
+ Real fCofactor10 = m_aafEntry[1][2] * m_aafEntry[2][0] -
+ m_aafEntry[1][0] * m_aafEntry[2][2];
+ Real fCofactor20 = m_aafEntry[1][0] * m_aafEntry[2][1] -
+ m_aafEntry[1][1] * m_aafEntry[2][0];
+
+ Real fDet =
+ m_aafEntry[0][0] * fCofactor00 +
+ m_aafEntry[0][1] * fCofactor10 +
+ m_aafEntry[0][2] * fCofactor20;
+
+ return fDet;
+}
+//----------------------------------------------------------------------------
+Matrix3 Matrix3::Slerp(Real fT, const Matrix3 &rkR0, const Matrix3 &rkR1)
+{
+ Vector3 kAxis;
+ Real fAngle;
+ Matrix3 kProd = rkR0.TransposeTimes(rkR1);
+ kProd.ToAxisAngle(kAxis, fAngle);
+
+ Matrix3 kR;
+ kR.FromAxisAngle(kAxis, fT * fAngle);
+
+ return kR;
+}
+//----------------------------------------------------------------------------
+void Matrix3::Bidiagonalize(Matrix3 &kA, Matrix3 &kL,
+ Matrix3 &kR)
+{
+ Real afV[3], afW[3];
+ Real fLength, fSign, fT1, fInvT1, fT2;
+ bool bIdentity;
+
+ // map first column to (*,0,0)
+ fLength = Math::Sqrt(kA[0][0] * kA[0][0] + kA[1][0] * kA[1][0] +
+ kA[2][0] * kA[2][0]);
+ if (fLength > 0.0f)
+ {
+ fSign = (kA[0][0] > 0.0f ? 1.0f : -1.0f);
+ fT1 = kA[0][0] + fSign * fLength;
+ fInvT1 = 1.0f / fT1;
+ afV[1] = kA[1][0] * fInvT1;
+ afV[2] = kA[2][0] * fInvT1;
+
+ fT2 = -2.0f / (1.0f + afV[1] * afV[1] + afV[2] * afV[2]);
+ afW[0] = fT2 * (kA[0][0] + kA[1][0] * afV[1] + kA[2][0] * afV[2]);
+ afW[1] = fT2 * (kA[0][1] + kA[1][1] * afV[1] + kA[2][1] * afV[2]);
+ afW[2] = fT2 * (kA[0][2] + kA[1][2] * afV[1] + kA[2][2] * afV[2]);
+ kA[0][0] += afW[0];
+ kA[0][1] += afW[1];
+ kA[0][2] += afW[2];
+ kA[1][1] += afV[1] * afW[1];
+ kA[1][2] += afV[1] * afW[2];
+ kA[2][1] += afV[2] * afW[1];
+ kA[2][2] += afV[2] * afW[2];
+
+ kL[0][0] = 1.0f + fT2;
+ kL[0][1] = kL[1][0] = fT2 * afV[1];
+ kL[0][2] = kL[2][0] = fT2 * afV[2];
+ kL[1][1] = 1.0f + fT2 * afV[1] * afV[1];
+ kL[1][2] = kL[2][1] = fT2 * afV[1] * afV[2];
+ kL[2][2] = 1.0f + fT2 * afV[2] * afV[2];
+ bIdentity = false;
+ }
+ else
+ {
+ kL = Matrix3::IDENTITY;
+ bIdentity = true;
+ }
+
+ // map first row to (*,*,0)
+ fLength = Math::Sqrt(kA[0][1] * kA[0][1] + kA[0][2] * kA[0][2]);
+ if (fLength > 0.0f)
+ {
+ fSign = (kA[0][1] > 0.0f ? 1.0f : -1.0f);
+ fT1 = kA[0][1] + fSign * fLength;
+ afV[2] = kA[0][2] / fT1;
+
+ fT2 = -2.0f / (1.0f + afV[2] * afV[2]);
+ afW[0] = fT2 * (kA[0][1] + kA[0][2] * afV[2]);
+ afW[1] = fT2 * (kA[1][1] + kA[1][2] * afV[2]);
+ afW[2] = fT2 * (kA[2][1] + kA[2][2] * afV[2]);
+ kA[0][1] += afW[0];
+ kA[1][1] += afW[1];
+ kA[1][2] += afW[1] * afV[2];
+ kA[2][1] += afW[2];
+ kA[2][2] += afW[2] * afV[2];
+
+ kR[0][0] = 1.0f;
+ kR[0][1] = kR[1][0] = 0.0f;
+ kR[0][2] = kR[2][0] = 0.0f;
+ kR[1][1] = 1.0f + fT2;
+ kR[1][2] = kR[2][1] = fT2 * afV[2];
+ kR[2][2] = 1.0f + fT2 * afV[2] * afV[2];
+ }
+ else
+ {
+ kR = Matrix3::IDENTITY;
+ }
+
+ // map second column to (*,*,0)
+ fLength = Math::Sqrt(kA[1][1] * kA[1][1] + kA[2][1] * kA[2][1]);
+ if (fLength > 0.0f)
+ {
+ fSign = (kA[1][1] > 0.0f ? 1.0f : -1.0f);
+ fT1 = kA[1][1] + fSign * fLength;
+ afV[2] = kA[2][1] / fT1;
+
+ fT2 = -2.0f / (1.0f + afV[2] * afV[2]);
+ afW[1] = fT2 * (kA[1][1] + kA[2][1] * afV[2]);
+ afW[2] = fT2 * (kA[1][2] + kA[2][2] * afV[2]);
+ kA[1][1] += afW[1];
+ kA[1][2] += afW[2];
+ kA[2][2] += afV[2] * afW[2];
+
+ Real fA = 1.0f + fT2;
+ Real fB = fT2 * afV[2];
+ Real fC = 1.0f + fB * afV[2];
+
+ if (bIdentity)
+ {
+ kL[0][0] = 1.0f;
+ kL[0][1] = kL[1][0] = 0.0f;
+ kL[0][2] = kL[2][0] = 0.0f;
+ kL[1][1] = fA;
+ kL[1][2] = kL[2][1] = fB;
+ kL[2][2] = fC;
+ }
+ else
+ {
+ for (int iRow = 0; iRow < 3; iRow++)
+ {
+ Real fTmp0 = kL[iRow][1];
+ Real fTmp1 = kL[iRow][2];
+ kL[iRow][1] = fA * fTmp0 + fB * fTmp1;
+ kL[iRow][2] = fB * fTmp0 + fC * fTmp1;
+ }
+ }
+ }
+}
+//----------------------------------------------------------------------------
+void Matrix3::GolubKahanStep(Matrix3 &kA, Matrix3 &kL,
+ Matrix3 &kR)
+{
+ Real fT11 = kA[0][1] * kA[0][1] + kA[1][1] * kA[1][1];
+ Real fT22 = kA[1][2] * kA[1][2] + kA[2][2] * kA[2][2];
+ Real fT12 = kA[1][1] * kA[1][2];
+ Real fTrace = fT11 + fT22;
+ Real fDiff = fT11 - fT22;
+ Real fDiscr = Math::Sqrt(fDiff * fDiff + 4.0f * fT12 * fT12);
+ Real fRoot1 = 0.5f * (fTrace + fDiscr);
+ Real fRoot2 = 0.5f * (fTrace - fDiscr);
+
+ // adjust right
+ Real fY = kA[0][0] - (Math::FAbs(fRoot1 - fT22) <=
+ Math::FAbs(fRoot2 - fT22)
+ ? fRoot1
+ : fRoot2);
+ Real fZ = kA[0][1];
+ Real fInvLength = Math::InvSqrt(fY * fY + fZ * fZ);
+ Real fSin = fZ * fInvLength;
+ Real fCos = -fY * fInvLength;
+
+ Real fTmp0 = kA[0][0];
+ Real fTmp1 = kA[0][1];
+ kA[0][0] = fCos * fTmp0 - fSin * fTmp1;
+ kA[0][1] = fSin * fTmp0 + fCos * fTmp1;
+ kA[1][0] = -fSin * kA[1][1];
+ kA[1][1] *= fCos;
+
+ int iRow;
+ for (iRow = 0; iRow < 3; iRow++)
+ {
+ fTmp0 = kR[0][iRow];
+ fTmp1 = kR[1][iRow];
+ kR[0][iRow] = fCos * fTmp0 - fSin * fTmp1;
+ kR[1][iRow] = fSin * fTmp0 + fCos * fTmp1;
+ }
+
+ // adjust left
+ fY = kA[0][0];
+ fZ = kA[1][0];
+ fInvLength = Math::InvSqrt(fY * fY + fZ * fZ);
+ fSin = fZ * fInvLength;
+ fCos = -fY * fInvLength;
+
+ kA[0][0] = fCos * kA[0][0] - fSin * kA[1][0];
+ fTmp0 = kA[0][1];
+ fTmp1 = kA[1][1];
+ kA[0][1] = fCos * fTmp0 - fSin * fTmp1;
+ kA[1][1] = fSin * fTmp0 + fCos * fTmp1;
+ kA[0][2] = -fSin * kA[1][2];
+ kA[1][2] *= fCos;
+
+ int iCol;
+ for (iCol = 0; iCol < 3; iCol++)
+ {
+ fTmp0 = kL[iCol][0];
+ fTmp1 = kL[iCol][1];
+ kL[iCol][0] = fCos * fTmp0 - fSin * fTmp1;
+ kL[iCol][1] = fSin * fTmp0 + fCos * fTmp1;
+ }
+
+ // adjust right
+ fY = kA[0][1];
+ fZ = kA[0][2];
+ fInvLength = Math::InvSqrt(fY * fY + fZ * fZ);
+ fSin = fZ * fInvLength;
+ fCos = -fY * fInvLength;
+
+ kA[0][1] = fCos * kA[0][1] - fSin * kA[0][2];
+ fTmp0 = kA[1][1];
+ fTmp1 = kA[1][2];
+ kA[1][1] = fCos * fTmp0 - fSin * fTmp1;
+ kA[1][2] = fSin * fTmp0 + fCos * fTmp1;
+ kA[2][1] = -fSin * kA[2][2];
+ kA[2][2] *= fCos;
+
+ for (iRow = 0; iRow < 3; iRow++)
+ {
+ fTmp0 = kR[1][iRow];
+ fTmp1 = kR[2][iRow];
+ kR[1][iRow] = fCos * fTmp0 - fSin * fTmp1;
+ kR[2][iRow] = fSin * fTmp0 + fCos * fTmp1;
+ }
+
+ // adjust left
+ fY = kA[1][1];
+ fZ = kA[2][1];
+ fInvLength = Math::InvSqrt(fY * fY + fZ * fZ);
+ fSin = fZ * fInvLength;
+ fCos = -fY * fInvLength;
+
+ kA[1][1] = fCos * kA[1][1] - fSin * kA[2][1];
+ fTmp0 = kA[1][2];
+ fTmp1 = kA[2][2];
+ kA[1][2] = fCos * fTmp0 - fSin * fTmp1;
+ kA[2][2] = fSin * fTmp0 + fCos * fTmp1;
+
+ for (iCol = 0; iCol < 3; iCol++)
+ {
+ fTmp0 = kL[iCol][1];
+ fTmp1 = kL[iCol][2];
+ kL[iCol][1] = fCos * fTmp0 - fSin * fTmp1;
+ kL[iCol][2] = fSin * fTmp0 + fCos * fTmp1;
+ }
+}
+//----------------------------------------------------------------------------
+void Matrix3::SingularValueDecomposition(Matrix3 &kL, Vector3 &kS,
+ Matrix3 &kR) const
+{
+ int iRow, iCol;
+
+ Matrix3 kA = *this;
+ Bidiagonalize(kA, kL, kR);
+
+ for (int i = 0; i < ms_iSvdMaxIterations; i++)
+ {
+ Real fTmp, fTmp0, fTmp1;
+ Real fSin0, fCos0, fTan0;
+ Real fSin1, fCos1, fTan1;
+
+ bool bTest1 = (Math::FAbs(kA[0][1]) <=
+ ms_fSvdEpsilon * (Math::FAbs(kA[0][0]) + Math::FAbs(kA[1][1])));
+ bool bTest2 = (Math::FAbs(kA[1][2]) <=
+ ms_fSvdEpsilon * (Math::FAbs(kA[1][1]) + Math::FAbs(kA[2][2])));
+ if (bTest1)
+ {
+ if (bTest2)
+ {
+ kS[0] = kA[0][0];
+ kS[1] = kA[1][1];
+ kS[2] = kA[2][2];
+ break;
+ }
+ else
+ {
+ // 2x2 closed form factorization
+ fTmp = (kA[1][1] * kA[1][1] - kA[2][2] * kA[2][2] +
+ kA[1][2] * kA[1][2]) /
+ (kA[1][2] * kA[2][2]);
+ fTan0 = 0.5f * (fTmp + Math::Sqrt(fTmp * fTmp + 4.0f));
+ fCos0 = Math::InvSqrt(1.0f + fTan0 * fTan0);
+ fSin0 = fTan0 * fCos0;
+
+ for (iCol = 0; iCol < 3; iCol++)
+ {
+ fTmp0 = kL[iCol][1];
+ fTmp1 = kL[iCol][2];
+ kL[iCol][1] = fCos0 * fTmp0 - fSin0 * fTmp1;
+ kL[iCol][2] = fSin0 * fTmp0 + fCos0 * fTmp1;
+ }
+
+ fTan1 = (kA[1][2] - kA[2][2] * fTan0) / kA[1][1];
+ fCos1 = Math::InvSqrt(1.0f + fTan1 * fTan1);
+ fSin1 = -fTan1 * fCos1;
+
+ for (iRow = 0; iRow < 3; iRow++)
+ {
+ fTmp0 = kR[1][iRow];
+ fTmp1 = kR[2][iRow];
+ kR[1][iRow] = fCos1 * fTmp0 - fSin1 * fTmp1;
+ kR[2][iRow] = fSin1 * fTmp0 + fCos1 * fTmp1;
+ }
+
+ kS[0] = kA[0][0];
+ kS[1] = fCos0 * fCos1 * kA[1][1] -
+ fSin1 * (fCos0 * kA[1][2] - fSin0 * kA[2][2]);
+ kS[2] = fSin0 * fSin1 * kA[1][1] +
+ fCos1 * (fSin0 * kA[1][2] + fCos0 * kA[2][2]);
+ break;
+ }
+ }
+ else
+ {
+ if (bTest2)
+ {
+ // 2x2 closed form factorization
+ fTmp = (kA[0][0] * kA[0][0] + kA[1][1] * kA[1][1] -
+ kA[0][1] * kA[0][1]) /
+ (kA[0][1] * kA[1][1]);
+ fTan0 = 0.5f * (-fTmp + Math::Sqrt(fTmp * fTmp + 4.0f));
+ fCos0 = Math::InvSqrt(1.0f + fTan0 * fTan0);
+ fSin0 = fTan0 * fCos0;
+
+ for (iCol = 0; iCol < 3; iCol++)
+ {
+ fTmp0 = kL[iCol][0];
+ fTmp1 = kL[iCol][1];
+ kL[iCol][0] = fCos0 * fTmp0 - fSin0 * fTmp1;
+ kL[iCol][1] = fSin0 * fTmp0 + fCos0 * fTmp1;
+ }
+
+ fTan1 = (kA[0][1] - kA[1][1] * fTan0) / kA[0][0];
+ fCos1 = Math::InvSqrt(1.0f + fTan1 * fTan1);
+ fSin1 = -fTan1 * fCos1;
+
+ for (iRow = 0; iRow < 3; iRow++)
+ {
+ fTmp0 = kR[0][iRow];
+ fTmp1 = kR[1][iRow];
+ kR[0][iRow] = fCos1 * fTmp0 - fSin1 * fTmp1;
+ kR[1][iRow] = fSin1 * fTmp0 + fCos1 * fTmp1;
+ }
+
+ kS[0] = fCos0 * fCos1 * kA[0][0] -
+ fSin1 * (fCos0 * kA[0][1] - fSin0 * kA[1][1]);
+ kS[1] = fSin0 * fSin1 * kA[0][0] +
+ fCos1 * (fSin0 * kA[0][1] + fCos0 * kA[1][1]);
+ kS[2] = kA[2][2];
+ break;
+ }
+ else
+ {
+ GolubKahanStep(kA, kL, kR);
+ }
+ }
+ }
+
+ // positize diagonal
+ for (iRow = 0; iRow < 3; iRow++)
+ {
+ if (kS[iRow] < 0.0f)
+ {
+ kS[iRow] = -kS[iRow];
+ for (iCol = 0; iCol < 3; iCol++)
+ kR[iRow][iCol] = -kR[iRow][iCol];
+ }
+ }
+}
+//----------------------------------------------------------------------------
+void Matrix3::SingularValueComposition(const Matrix3 &kL,
+ const Vector3 &kS, const Matrix3 &kR)
+{
+ int iRow, iCol;
+ Matrix3 kTmp;
+
+ // product S*R
+ for (iRow = 0; iRow < 3; iRow++)
+ {
+ for (iCol = 0; iCol < 3; iCol++)
+ kTmp[iRow][iCol] = kS[iRow] * kR[iRow][iCol];
+ }
+
+ // product L*S*R
+ for (iRow = 0; iRow < 3; iRow++)
+ {
+ for (iCol = 0; iCol < 3; iCol++)
+ {
+ m_aafEntry[iRow][iCol] = 0.0f;
+ for (int iMid = 0; iMid < 3; iMid++)
+ m_aafEntry[iRow][iCol] += kL[iRow][iMid] * kTmp[iMid][iCol];
+ }
+ }
+}
+//----------------------------------------------------------------------------
+void Matrix3::Orthonormalize()
+{
+ // Algorithm uses Gram-Schmidt orthogonalization. If 'this' matrix is
+ // M = [m0|m1|m2], then orthonormal output matrix is Q = [q0|q1|q2],
+ //
+ // q0 = m0/|m0|
+ // q1 = (m1-(q0*m1)q0)/|m1-(q0*m1)q0|
+ // q2 = (m2-(q0*m2)q0-(q1*m2)q1)/|m2-(q0*m2)q0-(q1*m2)q1|
+ //
+ // where |V| indicates length of vector V and A*B indicates dot
+ // product of vectors A and B.
+
+ // compute q0
+ Real fInvLength = Math::InvSqrt(m_aafEntry[0][0] * m_aafEntry[0][0] + m_aafEntry[1][0] * m_aafEntry[1][0] +
+ m_aafEntry[2][0] * m_aafEntry[2][0]);
+
+ m_aafEntry[0][0] *= fInvLength;
+ m_aafEntry[1][0] *= fInvLength;
+ m_aafEntry[2][0] *= fInvLength;
+
+ // compute q1
+ Real fDot0 =
+ m_aafEntry[0][0] * m_aafEntry[0][1] +
+ m_aafEntry[1][0] * m_aafEntry[1][1] +
+ m_aafEntry[2][0] * m_aafEntry[2][1];
+
+ m_aafEntry[0][1] -= fDot0 * m_aafEntry[0][0];
+ m_aafEntry[1][1] -= fDot0 * m_aafEntry[1][0];
+ m_aafEntry[2][1] -= fDot0 * m_aafEntry[2][0];
+
+ fInvLength = Math::InvSqrt(m_aafEntry[0][1] * m_aafEntry[0][1] +
+ m_aafEntry[1][1] * m_aafEntry[1][1] +
+ m_aafEntry[2][1] * m_aafEntry[2][1]);
+
+ m_aafEntry[0][1] *= fInvLength;
+ m_aafEntry[1][1] *= fInvLength;
+ m_aafEntry[2][1] *= fInvLength;
+
+ // compute q2
+ Real fDot1 =
+ m_aafEntry[0][1] * m_aafEntry[0][2] +
+ m_aafEntry[1][1] * m_aafEntry[1][2] +
+ m_aafEntry[2][1] * m_aafEntry[2][2];
+
+ fDot0 =
+ m_aafEntry[0][0] * m_aafEntry[0][2] +
+ m_aafEntry[1][0] * m_aafEntry[1][2] +
+ m_aafEntry[2][0] * m_aafEntry[2][2];
+
+ m_aafEntry[0][2] -= fDot0 * m_aafEntry[0][0] + fDot1 * m_aafEntry[0][1];
+ m_aafEntry[1][2] -= fDot0 * m_aafEntry[1][0] + fDot1 * m_aafEntry[1][1];
+ m_aafEntry[2][2] -= fDot0 * m_aafEntry[2][0] + fDot1 * m_aafEntry[2][1];
+
+ fInvLength = Math::InvSqrt(m_aafEntry[0][2] * m_aafEntry[0][2] +
+ m_aafEntry[1][2] * m_aafEntry[1][2] +
+ m_aafEntry[2][2] * m_aafEntry[2][2]);
+
+ m_aafEntry[0][2] *= fInvLength;
+ m_aafEntry[1][2] *= fInvLength;
+ m_aafEntry[2][2] *= fInvLength;
+}
+//----------------------------------------------------------------------------
+void Matrix3::QDUDecomposition(Matrix3 &kQ,
+ Vector3 &kD, Vector3 &kU) const
+{
+ // Factor M = QR = QDU where Q is orthogonal, D is diagonal,
+ // and U is upper triangular with ones on its diagonal. Algorithm uses
+ // Gram-Schmidt orthogonalization (the QR algorithm).
+ //
+ // If M = [ m0 | m1 | m2 ] and Q = [ q0 | q1 | q2 ], then
+ //
+ // q0 = m0/|m0|
+ // q1 = (m1-(q0*m1)q0)/|m1-(q0*m1)q0|
+ // q2 = (m2-(q0*m2)q0-(q1*m2)q1)/|m2-(q0*m2)q0-(q1*m2)q1|
+ //
+ // where |V| indicates length of vector V and A*B indicates dot
+ // product of vectors A and B. The matrix R has entries
+ //
+ // r00 = q0*m0 r01 = q0*m1 r02 = q0*m2
+ // r10 = 0 r11 = q1*m1 r12 = q1*m2
+ // r20 = 0 r21 = 0 r22 = q2*m2
+ //
+ // so D = diag(r00,r11,r22) and U has entries u01 = r01/r00,
+ // u02 = r02/r00, and u12 = r12/r11.
+
+ // Q = rotation
+ // D = scaling
+ // U = shear
+
+ // D stores the three diagonal entries r00, r11, r22
+ // U stores the entries U[0] = u01, U[1] = u02, U[2] = u12
+
+ // build orthogonal matrix Q
+ Real fInvLength = Math::InvSqrt(m_aafEntry[0][0] * m_aafEntry[0][0] + m_aafEntry[1][0] * m_aafEntry[1][0] +
+ m_aafEntry[2][0] * m_aafEntry[2][0]);
+ kQ[0][0] = m_aafEntry[0][0] * fInvLength;
+ kQ[1][0] = m_aafEntry[1][0] * fInvLength;
+ kQ[2][0] = m_aafEntry[2][0] * fInvLength;
+
+ Real fDot = kQ[0][0] * m_aafEntry[0][1] + kQ[1][0] * m_aafEntry[1][1] +
+ kQ[2][0] * m_aafEntry[2][1];
+ kQ[0][1] = m_aafEntry[0][1] - fDot * kQ[0][0];
+ kQ[1][1] = m_aafEntry[1][1] - fDot * kQ[1][0];
+ kQ[2][1] = m_aafEntry[2][1] - fDot * kQ[2][0];
+ fInvLength = Math::InvSqrt(kQ[0][1] * kQ[0][1] + kQ[1][1] * kQ[1][1] +
+ kQ[2][1] * kQ[2][1]);
+ kQ[0][1] *= fInvLength;
+ kQ[1][1] *= fInvLength;
+ kQ[2][1] *= fInvLength;
+
+ fDot = kQ[0][0] * m_aafEntry[0][2] + kQ[1][0] * m_aafEntry[1][2] +
+ kQ[2][0] * m_aafEntry[2][2];
+ kQ[0][2] = m_aafEntry[0][2] - fDot * kQ[0][0];
+ kQ[1][2] = m_aafEntry[1][2] - fDot * kQ[1][0];
+ kQ[2][2] = m_aafEntry[2][2] - fDot * kQ[2][0];
+ fDot = kQ[0][1] * m_aafEntry[0][2] + kQ[1][1] * m_aafEntry[1][2] +
+ kQ[2][1] * m_aafEntry[2][2];
+ kQ[0][2] -= fDot * kQ[0][1];
+ kQ[1][2] -= fDot * kQ[1][1];
+ kQ[2][2] -= fDot * kQ[2][1];
+ fInvLength = Math::InvSqrt(kQ[0][2] * kQ[0][2] + kQ[1][2] * kQ[1][2] +
+ kQ[2][2] * kQ[2][2]);
+ kQ[0][2] *= fInvLength;
+ kQ[1][2] *= fInvLength;
+ kQ[2][2] *= fInvLength;
+
+ // guarantee that orthogonal matrix has determinant 1 (no reflections)
+ Real fDet = kQ[0][0] * kQ[1][1] * kQ[2][2] + kQ[0][1] * kQ[1][2] * kQ[2][0] +
+ kQ[0][2] * kQ[1][0] * kQ[2][1] - kQ[0][2] * kQ[1][1] * kQ[2][0] -
+ kQ[0][1] * kQ[1][0] * kQ[2][2] - kQ[0][0] * kQ[1][2] * kQ[2][1];
+
+ if (fDet < 0.0f)
+ {
+ for (int iRow = 0; iRow < 3; iRow++)
+ {
+ for (int iCol = 0; iCol < 3; iCol++)
+ kQ[iRow][iCol] = -kQ[iRow][iCol];
+ }
+ }
+
+ // build "right" matrix R
+ Matrix3 kR;
+ kR[0][0] = kQ[0][0] * m_aafEntry[0][0] + kQ[1][0] * m_aafEntry[1][0] +
+ kQ[2][0] * m_aafEntry[2][0];
+ kR[0][1] = kQ[0][0] * m_aafEntry[0][1] + kQ[1][0] * m_aafEntry[1][1] +
+ kQ[2][0] * m_aafEntry[2][1];
+ kR[1][1] = kQ[0][1] * m_aafEntry[0][1] + kQ[1][1] * m_aafEntry[1][1] +
+ kQ[2][1] * m_aafEntry[2][1];
+ kR[0][2] = kQ[0][0] * m_aafEntry[0][2] + kQ[1][0] * m_aafEntry[1][2] +
+ kQ[2][0] * m_aafEntry[2][2];
+ kR[1][2] = kQ[0][1] * m_aafEntry[0][2] + kQ[1][1] * m_aafEntry[1][2] +
+ kQ[2][1] * m_aafEntry[2][2];
+ kR[2][2] = kQ[0][2] * m_aafEntry[0][2] + kQ[1][2] * m_aafEntry[1][2] +
+ kQ[2][2] * m_aafEntry[2][2];
+
+ // the scaling component
+ kD[0] = kR[0][0];
+ kD[1] = kR[1][1];
+ kD[2] = kR[2][2];
+
+ // the shear component
+ Real fInvD0 = 1.0f / kD[0];
+ kU[0] = kR[0][1] * fInvD0;
+ kU[1] = kR[0][2] * fInvD0;
+ kU[2] = kR[1][2] / kD[1];
+}
+//----------------------------------------------------------------------------
+Real Matrix3::MaxCubicRoot(Real afCoeff[3])
+{
+ // Spectral norm is for A^T*A, so characteristic polynomial
+ // P(x) = c[0]+c[1]*x+c[2]*x^2+x^3 has three positive real roots.
+ // This yields the assertions c[0] < 0 and c[2]*c[2] >= 3*c[1].
+
+ // quick out for uniform scale (triple root)
+ const Real fOneThird = 1.0f / 3.0f;
+ const Real fEpsilon = 1e-06f;
+ Real fDiscr = afCoeff[2] * afCoeff[2] - 3.0f * afCoeff[1];
+ if (fDiscr <= fEpsilon)
+ return -fOneThird * afCoeff[2];
+
+ // Compute an upper bound on roots of P(x). This assumes that A^T*A
+ // has been scaled by its largest entry.
+ Real fX = 1.0f;
+ Real fPoly = afCoeff[0] + fX * (afCoeff[1] + fX * (afCoeff[2] + fX));
+ if (fPoly < 0.0f)
+ {
+ // uses a matrix norm to find an upper bound on maximum root
+ fX = Math::FAbs(afCoeff[0]);
+ Real fTmp = 1.0f + Math::FAbs(afCoeff[1]);
+ if (fTmp > fX)
+ fX = fTmp;
+ fTmp = 1.0f + Math::FAbs(afCoeff[2]);
+ if (fTmp > fX)
+ fX = fTmp;
+ }
+
+ // Newton's method to find root
+ Real fTwoC2 = 2.0f * afCoeff[2];
+ for (int i = 0; i < 16; i++)
+ {
+ fPoly = afCoeff[0] + fX * (afCoeff[1] + fX * (afCoeff[2] + fX));
+ if (Math::FAbs(fPoly) <= fEpsilon)
+ return fX;
+
+ Real fDeriv = afCoeff[1] + fX * (fTwoC2 + 3.0f * fX);
+ fX -= fPoly / fDeriv;
+ }
+
+ return fX;
+}
+//----------------------------------------------------------------------------
+Real Matrix3::SpectralNorm() const
+{
+ Matrix3 kP;
+ int iRow, iCol;
+ Real fPmax = 0.0f;
+ for (iRow = 0; iRow < 3; iRow++)
+ {
+ for (iCol = 0; iCol < 3; iCol++)
+ {
+ kP[iRow][iCol] = 0.0f;
+ for (int iMid = 0; iMid < 3; iMid++)
+ {
+ kP[iRow][iCol] +=
+ m_aafEntry[iMid][iRow] * m_aafEntry[iMid][iCol];
+ }
+ if (kP[iRow][iCol] > fPmax)
+ fPmax = kP[iRow][iCol];
+ }
+ }
+
+ Real fInvPmax = 1.0f / fPmax;
+ for (iRow = 0; iRow < 3; iRow++)
+ {
+ for (iCol = 0; iCol < 3; iCol++)
+ kP[iRow][iCol] *= fInvPmax;
+ }
+
+ Real afCoeff[3];
+ afCoeff[0] = -(kP[0][0] * (kP[1][1] * kP[2][2] - kP[1][2] * kP[2][1]) +
+ kP[0][1] * (kP[2][0] * kP[1][2] - kP[1][0] * kP[2][2]) +
+ kP[0][2] * (kP[1][0] * kP[2][1] - kP[2][0] * kP[1][1]));
+ afCoeff[1] = kP[0][0] * kP[1][1] - kP[0][1] * kP[1][0] +
+ kP[0][0] * kP[2][2] - kP[0][2] * kP[2][0] +
+ kP[1][1] * kP[2][2] - kP[1][2] * kP[2][1];
+ afCoeff[2] = -(kP[0][0] + kP[1][1] + kP[2][2]);
+
+ Real fRoot = MaxCubicRoot(afCoeff);
+ Real fNorm = Math::Sqrt(fPmax * fRoot);
+ return fNorm;
+}
+//----------------------------------------------------------------------------
+void Matrix3::ToAxisAngle(Vector3 &rkAxis, Real &rfRadians) const
+{
+ // Let (x,y,z) be the unit-length axis and let A be an angle of rotation.
+ // The rotation matrix is R = I + sin(A)*P + (1-cos(A))*P^2 where
+ // I is the identity and
+ //
+ // +- -+
+ // P = | 0 -z +y |
+ // | +z 0 -x |
+ // | -y +x 0 |
+ // +- -+
+ //
+ // If A > 0, R represents a counterclockwise rotation about the axis in
+ // the sense of looking from the tip of the axis vector towards the
+ // origin. Some algebra will show that
+ //
+ // cos(A) = (trace(R)-1)/2 and R - R^t = 2*sin(A)*P
+ //
+ // In the event that A = pi, R-R^t = 0 which prevents us from extracting
+ // the axis through P. Instead note that R = I+2*P^2 when A = pi, so
+ // P^2 = (R-I)/2. The diagonal entries of P^2 are x^2-1, y^2-1, and
+ // z^2-1. We can solve these for axis (x,y,z). Because the angle is pi,
+ // it does not matter which sign you choose on the square roots.
+
+ Real fTrace = m_aafEntry[0][0] + m_aafEntry[1][1] + m_aafEntry[2][2];
+ Real fCos = 0.5f * (fTrace - 1.0f);
+ rfRadians = Math::ACos(fCos); // in [0,PI]
+
+ if (rfRadians > 0.0f)
+ {
+ if (rfRadians < Math::_PI)
+ {
+ rkAxis.x = m_aafEntry[2][1] - m_aafEntry[1][2];
+ rkAxis.y = m_aafEntry[0][2] - m_aafEntry[2][0];
+ rkAxis.z = m_aafEntry[1][0] - m_aafEntry[0][1];
+ rkAxis.Unitize();
+ }
+ else
+ {
+ // angle is PI
+ Real fHalfInverse;
+ if (m_aafEntry[0][0] >= m_aafEntry[1][1])
+ {
+ // r00 >= r11
+ if (m_aafEntry[0][0] >= m_aafEntry[2][2])
+ {
+ // r00 is maximum diagonal term
+ rkAxis.x = 0.5f * Math::Sqrt(m_aafEntry[0][0] -
+ m_aafEntry[1][1] - m_aafEntry[2][2] + 1.0f);
+ fHalfInverse = 0.5f / rkAxis.x;
+ rkAxis.y = fHalfInverse * m_aafEntry[0][1];
+ rkAxis.z = fHalfInverse * m_aafEntry[0][2];
+ }
+ else
+ {
+ // r22 is maximum diagonal term
+ rkAxis.z = 0.5f * Math::Sqrt(m_aafEntry[2][2] -
+ m_aafEntry[0][0] - m_aafEntry[1][1] + 1.0f);
+ fHalfInverse = 0.5f / rkAxis.z;
+ rkAxis.x = fHalfInverse * m_aafEntry[0][2];
+ rkAxis.y = fHalfInverse * m_aafEntry[1][2];
+ }
+ }
+ else
+ {
+ // r11 > r00
+ if (m_aafEntry[1][1] >= m_aafEntry[2][2])
+ {
+ // r11 is maximum diagonal term
+ rkAxis.y = 0.5f * Math::Sqrt(m_aafEntry[1][1] -
+ m_aafEntry[0][0] - m_aafEntry[2][2] + 1.0f);
+ fHalfInverse = 0.5f / rkAxis.y;
+ rkAxis.x = fHalfInverse * m_aafEntry[0][1];
+ rkAxis.z = fHalfInverse * m_aafEntry[1][2];
+ }
+ else
+ {
+ // r22 is maximum diagonal term
+ rkAxis.z = 0.5f * Math::Sqrt(m_aafEntry[2][2] -
+ m_aafEntry[0][0] - m_aafEntry[1][1] + 1.0f);
+ fHalfInverse = 0.5f / rkAxis.z;
+ rkAxis.x = fHalfInverse * m_aafEntry[0][2];
+ rkAxis.y = fHalfInverse * m_aafEntry[1][2];
+ }
+ }
+ }
+ }
+ else
+ {
+ // The angle is 0 and the matrix is the identity. Any axis will
+ // work, so just use the x-axis.
+ rkAxis.x = 1.0f;
+ rkAxis.y = 0.0f;
+ rkAxis.z = 0.0f;
+ }
+}
+//----------------------------------------------------------------------------
+void Matrix3::FromAxisAngle(const Vector3 &rkAxis, Real fRadians)
+{
+ Real fCos = Math::Cos(fRadians);
+ Real fSin = Math::Sin(fRadians);
+ Real fOneMinusCos = 1.0f - fCos;
+ Real fX2 = rkAxis.x * rkAxis.x;
+ Real fY2 = rkAxis.y * rkAxis.y;
+ Real fZ2 = rkAxis.z * rkAxis.z;
+ Real fXYM = rkAxis.x * rkAxis.y * fOneMinusCos;
+ Real fXZM = rkAxis.x * rkAxis.z * fOneMinusCos;
+ Real fYZM = rkAxis.y * rkAxis.z * fOneMinusCos;
+ Real fXSin = rkAxis.x * fSin;
+ Real fYSin = rkAxis.y * fSin;
+ Real fZSin = rkAxis.z * fSin;
+
+ m_aafEntry[0][0] = fX2 * fOneMinusCos + fCos;
+ m_aafEntry[0][1] = fXYM - fZSin;
+ m_aafEntry[0][2] = fXZM + fYSin;
+ m_aafEntry[1][0] = fXYM + fZSin;
+ m_aafEntry[1][1] = fY2 * fOneMinusCos + fCos;
+ m_aafEntry[1][2] = fYZM - fXSin;
+ m_aafEntry[2][0] = fXZM - fYSin;
+ m_aafEntry[2][1] = fYZM + fXSin;
+ m_aafEntry[2][2] = fZ2 * fOneMinusCos + fCos;
+}
+//----------------------------------------------------------------------------
+bool Matrix3::ToEulerAnglesXYZ(Real &rfXAngle, Real &rfYAngle,
+ Real &rfZAngle) const
+{
+ // rot = cy*cz -cy*sz sy
+ // cz*sx*sy+cx*sz cx*cz-sx*sy*sz -cy*sx
+ // -cx*cz*sy+sx*sz cz*sx+cx*sy*sz cx*cy
+
+ if (m_aafEntry[0][2] < 1.0f)
+ {
+ if (m_aafEntry[0][2] > -1.0f)
+ {
+ rfXAngle = Math::ATan2(-m_aafEntry[1][2], m_aafEntry[2][2]);
+ rfYAngle = (Real)asin(m_aafEntry[0][2]);
+ rfZAngle = Math::ATan2(-m_aafEntry[0][1], m_aafEntry[0][0]);
+ return true;
+ }
+ else
+ {
+ // WARNING. Not unique. XA - ZA = -atan2(r10,r11)
+ rfXAngle = -Math::ATan2(m_aafEntry[1][0], m_aafEntry[1][1]);
+ rfYAngle = -Math::HALF_PI;
+ rfZAngle = 0.0f;
+ return false;
+ }
+ }
+ else
+ {
+ // WARNING. Not unique. XAngle + ZAngle = atan2(r10,r11)
+ rfXAngle = Math::ATan2(m_aafEntry[1][0], m_aafEntry[1][1]);
+ rfYAngle = Math::HALF_PI;
+ rfZAngle = 0.0f;
+ return false;
+ }
+}
+//----------------------------------------------------------------------------
+bool Matrix3::ToEulerAnglesXZY(Real &rfXAngle, Real &rfZAngle,
+ Real &rfYAngle) const
+{
+ // rot = cy*cz -sz cz*sy
+ // sx*sy+cx*cy*sz cx*cz -cy*sx+cx*sy*sz
+ // -cx*sy+cy*sx*sz cz*sx cx*cy+sx*sy*sz
+
+ if (m_aafEntry[0][1] < 1.0f)
+ {
+ if (m_aafEntry[0][1] > -1.0f)
+ {
+ rfXAngle = Math::ATan2(m_aafEntry[2][1], m_aafEntry[1][1]);
+ rfZAngle = (Real)asin(-m_aafEntry[0][1]);
+ rfYAngle = Math::ATan2(m_aafEntry[0][2], m_aafEntry[0][0]);
+ return true;
+ }
+ else
+ {
+ // WARNING. Not unique. XA - YA = atan2(r20,r22)
+ rfXAngle = Math::ATan2(m_aafEntry[2][0], m_aafEntry[2][2]);
+ rfZAngle = Math::HALF_PI;
+ rfYAngle = 0.0f;
+ return false;
+ }
+ }
+ else
+ {
+ // WARNING. Not unique. XA + YA = atan2(-r20,r22)
+ rfXAngle = Math::ATan2(-m_aafEntry[2][0], m_aafEntry[2][2]);
+ rfZAngle = -Math::HALF_PI;
+ rfYAngle = 0.0f;
+ return false;
+ }
+}
+//----------------------------------------------------------------------------
+bool Matrix3::ToEulerAnglesYXZ(Real &rfYAngle, Real &rfXAngle,
+ Real &rfZAngle) const
+{
+ // rot = cy*cz+sx*sy*sz cz*sx*sy-cy*sz cx*sy
+ // cx*sz cx*cz -sx
+ // -cz*sy+cy*sx*sz cy*cz*sx+sy*sz cx*cy
+
+ if (m_aafEntry[1][2] < 1.0f)
+ {
+ if (m_aafEntry[1][2] > -1.0f)
+ {
+ rfYAngle = Math::ATan2(m_aafEntry[0][2], m_aafEntry[2][2]);
+ rfXAngle = (Real)asin(-m_aafEntry[1][2]);
+ rfZAngle = Math::ATan2(m_aafEntry[1][0], m_aafEntry[1][1]);
+ return true;
+ }
+ else
+ {
+ // WARNING. Not unique. YA - ZA = atan2(r01,r00)
+ rfYAngle = Math::ATan2(m_aafEntry[0][1], m_aafEntry[0][0]);
+ rfXAngle = Math::HALF_PI;
+ rfZAngle = 0.0f;
+ return false;
+ }
+ }
+ else
+ {
+ // WARNING. Not unique. YA + ZA = atan2(-r01,r00)
+ rfYAngle = Math::ATan2(-m_aafEntry[0][1], m_aafEntry[0][0]);
+ rfXAngle = -Math::HALF_PI;
+ rfZAngle = 0.0f;
+ return false;
+ }
+}
+//----------------------------------------------------------------------------
+bool Matrix3::ToEulerAnglesYZX(Real &rfYAngle, Real &rfZAngle,
+ Real &rfXAngle) const
+{
+ // rot = cy*cz sx*sy-cx*cy*sz cx*sy+cy*sx*sz
+ // sz cx*cz -cz*sx
+ // -cz*sy cy*sx+cx*sy*sz cx*cy-sx*sy*sz
+
+ if (m_aafEntry[1][0] < 1.0f)
+ {
+ if (m_aafEntry[1][0] > -1.0f)
+ {
+ rfYAngle = Math::ATan2(-m_aafEntry[2][0], m_aafEntry[0][0]);
+ rfZAngle = (Real)asin(m_aafEntry[1][0]);
+ rfXAngle = Math::ATan2(-m_aafEntry[1][2], m_aafEntry[1][1]);
+ return true;
+ }
+ else
+ {
+ // WARNING. Not unique. YA - XA = -atan2(r21,r22);
+ rfYAngle = -Math::ATan2(m_aafEntry[2][1], m_aafEntry[2][2]);
+ rfZAngle = -Math::HALF_PI;
+ rfXAngle = 0.0f;
+ return false;
+ }
+ }
+ else
+ {
+ // WARNING. Not unique. YA + XA = atan2(r21,r22)
+ rfYAngle = Math::ATan2(m_aafEntry[2][1], m_aafEntry[2][2]);
+ rfZAngle = Math::HALF_PI;
+ rfXAngle = 0.0f;
+ return false;
+ }
+}
+//----------------------------------------------------------------------------
+bool Matrix3::ToEulerAnglesZXY(Real &rfZAngle, Real &rfXAngle,
+ Real &rfYAngle) const
+{
+ // rot = cy*cz-sx*sy*sz -cx*sz cz*sy+cy*sx*sz
+ // cz*sx*sy+cy*sz cx*cz -cy*cz*sx+sy*sz
+ // -cx*sy sx cx*cy
+
+ if (m_aafEntry[2][1] < 1.0f)
+ {
+ if (m_aafEntry[2][1] > -1.0f)
+ {
+ rfZAngle = Math::ATan2(-m_aafEntry[0][1], m_aafEntry[1][1]);
+ rfXAngle = (Real)asin(m_aafEntry[2][1]);
+ rfYAngle = Math::ATan2(-m_aafEntry[2][0], m_aafEntry[2][2]);
+ return true;
+ }
+ else
+ {
+ // WARNING. Not unique. ZA - YA = -atan(r02,r00)
+ rfZAngle = -Math::ATan2(m_aafEntry[0][2], m_aafEntry[0][0]);
+ rfXAngle = -Math::HALF_PI;
+ rfYAngle = 0.0f;
+ return false;
+ }
+ }
+ else
+ {
+ // WARNING. Not unique. ZA + YA = atan2(r02,r00)
+ rfZAngle = Math::ATan2(m_aafEntry[0][2], m_aafEntry[0][0]);
+ rfXAngle = Math::HALF_PI;
+ rfYAngle = 0.0f;
+ return false;
+ }
+}
+//----------------------------------------------------------------------------
+bool Matrix3::ToEulerAnglesZYX(Real &rfZAngle, Real &rfYAngle,
+ Real &rfXAngle) const
+{
+ // rot = cy*cz cz*sx*sy-cx*sz cx*cz*sy+sx*sz
+ // cy*sz cx*cz+sx*sy*sz -cz*sx+cx*sy*sz
+ // -sy cy*sx cx*cy
+
+ if (m_aafEntry[2][0] < 1.0f)
+ {
+ if (m_aafEntry[2][0] > -1.0f)
+ {
+ rfZAngle = Math::ATan2(m_aafEntry[1][0], m_aafEntry[0][0]);
+ rfYAngle = (Real)asin(-m_aafEntry[2][0]);
+ rfXAngle = Math::ATan2(m_aafEntry[2][1], m_aafEntry[2][2]);
+ return true;
+ }
+ else
+ {
+ // WARNING. Not unique. ZA - XA = -atan2(r01,r02)
+ rfZAngle = -Math::ATan2(m_aafEntry[0][1], m_aafEntry[0][2]);
+ rfYAngle = Math::HALF_PI;
+ rfXAngle = 0.0f;
+ return false;
+ }
+ }
+ else
+ {
+ // WARNING. Not unique. ZA + XA = atan2(-r01,-r02)
+ rfZAngle = Math::ATan2(-m_aafEntry[0][1], -m_aafEntry[0][2]);
+ rfYAngle = -Math::HALF_PI;
+ rfXAngle = 0.0f;
+ return false;
+ }
+}
+//----------------------------------------------------------------------------
+void Matrix3::FromEulerAnglesXYZ(Real fYAngle, Real fPAngle,
+ Real fRAngle)
+{
+ Real fCos, fSin;
+
+ fCos = Math::Cos(fYAngle);
+ fSin = Math::Sin(fYAngle);
+ Matrix3 kXMat(1.0f, 0.0f, 0.0f, 0.0f, fCos, -fSin, 0.0f, fSin, fCos);
+
+ fCos = Math::Cos(fPAngle);
+ fSin = Math::Sin(fPAngle);
+ Matrix3 kYMat(fCos, 0.0f, fSin, 0.0f, 1.0f, 0.0f, -fSin, 0.0f, fCos);
+
+ fCos = Math::Cos(fRAngle);
+ fSin = Math::Sin(fRAngle);
+ Matrix3 kZMat(fCos, -fSin, 0.0f, fSin, fCos, 0.0f, 0.0f, 0.0f, 1.0f);
+
+ *this = kXMat * (kYMat * kZMat);
+}
+//----------------------------------------------------------------------------
+void Matrix3::FromEulerAnglesXZY(Real fYAngle, Real fPAngle,
+ Real fRAngle)
+{
+ Real fCos, fSin;
+
+ fCos = Math::Cos(fYAngle);
+ fSin = Math::Sin(fYAngle);
+ Matrix3 kXMat(1.0f, 0.0f, 0.0f, 0.0f, fCos, -fSin, 0.0f, fSin, fCos);
+
+ fCos = Math::Cos(fPAngle);
+ fSin = Math::Sin(fPAngle);
+ Matrix3 kZMat(fCos, -fSin, 0.0f, fSin, fCos, 0.0f, 0.0f, 0.0f, 1.0f);
+
+ fCos = Math::Cos(fRAngle);
+ fSin = Math::Sin(fRAngle);
+ Matrix3 kYMat(fCos, 0.0f, fSin, 0.0f, 1.0f, 0.0f, -fSin, 0.0f, fCos);
+
+ *this = kXMat * (kZMat * kYMat);
+}
+//----------------------------------------------------------------------------
+void Matrix3::FromEulerAnglesYXZ(Real fYAngle, Real fPAngle,
+ Real fRAngle)
+{
+ Real fCos, fSin;
+
+ fCos = Math::Cos(fYAngle);
+ fSin = Math::Sin(fYAngle);
+ Matrix3 kYMat(fCos, 0.0f, fSin, 0.0f, 1.0f, 0.0f, -fSin, 0.0f, fCos);
+
+ fCos = Math::Cos(fPAngle);
+ fSin = Math::Sin(fPAngle);
+ Matrix3 kXMat(1.0f, 0.0f, 0.0f, 0.0f, fCos, -fSin, 0.0f, fSin, fCos);
+
+ fCos = Math::Cos(fRAngle);
+ fSin = Math::Sin(fRAngle);
+ Matrix3 kZMat(fCos, -fSin, 0.0f, fSin, fCos, 0.0f, 0.0f, 0.0f, 1.0f);
+
+ *this = kYMat * (kXMat * kZMat);
+}
+//----------------------------------------------------------------------------
+void Matrix3::FromEulerAnglesYZX(Real fYAngle, Real fPAngle,
+ Real fRAngle)
+{
+ Real fCos, fSin;
+
+ fCos = Math::Cos(fYAngle);
+ fSin = Math::Sin(fYAngle);
+ Matrix3 kYMat(fCos, 0.0f, fSin, 0.0f, 1.0f, 0.0f, -fSin, 0.0f, fCos);
+
+ fCos = Math::Cos(fPAngle);
+ fSin = Math::Sin(fPAngle);
+ Matrix3 kZMat(fCos, -fSin, 0.0f, fSin, fCos, 0.0f, 0.0f, 0.0f, 1.0f);
+
+ fCos = Math::Cos(fRAngle);
+ fSin = Math::Sin(fRAngle);
+ Matrix3 kXMat(1.0f, 0.0f, 0.0f, 0.0f, fCos, -fSin, 0.0f, fSin, fCos);
+
+ *this = kYMat * (kZMat * kXMat);
+}
+//----------------------------------------------------------------------------
+void Matrix3::FromEulerAnglesZXY(Real fYAngle, Real fPAngle,
+ Real fRAngle)
+{
+ Real fCos, fSin;
+
+ fCos = Math::Cos(fYAngle);
+ fSin = Math::Sin(fYAngle);
+ Matrix3 kZMat(fCos, -fSin, 0.0f, fSin, fCos, 0.0f, 0.0f, 0.0f, 1.0f);
+
+ fCos = Math::Cos(fPAngle);
+ fSin = Math::Sin(fPAngle);
+ Matrix3 kXMat(1.0f, 0.0f, 0.0f, 0.0f, fCos, -fSin, 0.0f, fSin, fCos);
+
+ fCos = Math::Cos(fRAngle);
+ fSin = Math::Sin(fRAngle);
+ Matrix3 kYMat(fCos, 0.0f, fSin, 0.0f, 1.0f, 0.0f, -fSin, 0.0f, fCos);
+
+ *this = kZMat * (kXMat * kYMat);
+}
+//----------------------------------------------------------------------------
+void Matrix3::FromEulerAnglesZYX(Real fYAngle, Real fPAngle,
+ Real fRAngle)
+{
+ Real fCos, fSin;
+
+ fCos = Math::Cos(fYAngle);
+ fSin = Math::Sin(fYAngle);
+ Matrix3 kZMat(fCos, -fSin, 0.0f, fSin, fCos, 0.0f, 0.0f, 0.0f, 1.0f);
+
+ fCos = Math::Cos(fPAngle);
+ fSin = Math::Sin(fPAngle);
+ Matrix3 kYMat(fCos, 0.0f, fSin, 0.0f, 1.0f, 0.0f, -fSin, 0.0f, fCos);
+
+ fCos = Math::Cos(fRAngle);
+ fSin = Math::Sin(fRAngle);
+ Matrix3 kXMat(1.0f, 0.0f, 0.0f, 0.0f, fCos, -fSin, 0.0f, fSin, fCos);
+
+ *this = kZMat * (kYMat * kXMat);
+}
+//----------------------------------------------------------------------------
+void Matrix3::Tridiagonal(Real afDiag[3], Real afSubDiag[3])
+{
+ // Householder reduction T = Q^t M Q
+ // Input:
+ // mat, symmetric 3x3 matrix M
+ // Output:
+ // mat, orthogonal matrix Q
+ // diag, diagonal entries of T
+ // subd, subdiagonal entries of T (T is symmetric)
+
+ Real fA = m_aafEntry[0][0];
+ Real fB = m_aafEntry[0][1];
+ Real fC = m_aafEntry[0][2];
+ Real fD = m_aafEntry[1][1];
+ Real fE = m_aafEntry[1][2];
+ Real fF = m_aafEntry[2][2];
+
+ afDiag[0] = fA;
+ afSubDiag[2] = 0.0f;
+ if (Math::FAbs(fC) >= EPSILON)
+ {
+ Real fLength = Math::Sqrt(fB * fB + fC * fC);
+ Real fInvLength = 1.0f / fLength;
+ fB *= fInvLength;
+ fC *= fInvLength;
+ Real fQ = 2.0f * fB * fE + fC * (fF - fD);
+ afDiag[1] = fD + fC * fQ;
+ afDiag[2] = fF - fC * fQ;
+ afSubDiag[0] = fLength;
+ afSubDiag[1] = fE - fB * fQ;
+ m_aafEntry[0][0] = 1.0f;
+ m_aafEntry[0][1] = 0.0f;
+ m_aafEntry[0][2] = 0.0f;
+ m_aafEntry[1][0] = 0.0f;
+ m_aafEntry[1][1] = fB;
+ m_aafEntry[1][2] = fC;
+ m_aafEntry[2][0] = 0.0f;
+ m_aafEntry[2][1] = fC;
+ m_aafEntry[2][2] = -fB;
+ }
+ else
+ {
+ afDiag[1] = fD;
+ afDiag[2] = fF;
+ afSubDiag[0] = fB;
+ afSubDiag[1] = fE;
+ m_aafEntry[0][0] = 1.0f;
+ m_aafEntry[0][1] = 0.0f;
+ m_aafEntry[0][2] = 0.0f;
+ m_aafEntry[1][0] = 0.0f;
+ m_aafEntry[1][1] = 1.0f;
+ m_aafEntry[1][2] = 0.0f;
+ m_aafEntry[2][0] = 0.0f;
+ m_aafEntry[2][1] = 0.0f;
+ m_aafEntry[2][2] = 1.0f;
+ }
+}
+//----------------------------------------------------------------------------
+bool Matrix3::QLAlgorithm(Real afDiag[3], Real afSubDiag[3])
+{
+ // QL iteration with implicit shifting to reduce matrix from tridiagonal
+ // to diagonal
+
+ for (int i0 = 0; i0 < 3; i0++)
+ {
+ const int iMaxIter = 32;
+ int iIter;
+ for (iIter = 0; iIter < iMaxIter; iIter++)
+ {
+ int i1;
+ for (i1 = i0; i1 <= 1; i1++)
+ {
+ Real fSum = Math::FAbs(afDiag[i1]) +
+ Math::FAbs(afDiag[i1 + 1]);
+ if (Math::FAbs(afSubDiag[i1]) + fSum == fSum)
+ break;
+ }
+ if (i1 == i0)
+ break;
+
+ Real fTmp0 = (afDiag[i0 + 1] - afDiag[i0]) / (2.0f * afSubDiag[i0]);
+ Real fTmp1 = Math::Sqrt(fTmp0 * fTmp0 + 1.0f);
+ if (fTmp0 < 0.0f)
+ fTmp0 = afDiag[i1] - afDiag[i0] + afSubDiag[i0] / (fTmp0 - fTmp1);
+ else
+ fTmp0 = afDiag[i1] - afDiag[i0] + afSubDiag[i0] / (fTmp0 + fTmp1);
+ Real fSin = 1.0f;
+ Real fCos = 1.0f;
+ Real fTmp2 = 0.0f;
+ for (int i2 = i1 - 1; i2 >= i0; i2--)
+ {
+ Real fTmp3 = fSin * afSubDiag[i2];
+ Real fTmp4 = fCos * afSubDiag[i2];
+ if (Math::FAbs(fTmp3) >= Math::FAbs(fTmp0))
+ {
+ fCos = fTmp0 / fTmp3;
+ fTmp1 = Math::Sqrt(fCos * fCos + 1.0f);
+ afSubDiag[i2 + 1] = fTmp3 * fTmp1;
+ fSin = 1.0f / fTmp1;
+ fCos *= fSin;
+ }
+ else
+ {
+ fSin = fTmp3 / fTmp0;
+ fTmp1 = Math::Sqrt(fSin * fSin + 1.0f);
+ afSubDiag[i2 + 1] = fTmp0 * fTmp1;
+ fCos = 1.0f / fTmp1;
+ fSin *= fCos;
+ }
+ fTmp0 = afDiag[i2 + 1] - fTmp2;
+ fTmp1 = (afDiag[i2] - fTmp0) * fSin + 2.0f * fTmp4 * fCos;
+ fTmp2 = fSin * fTmp1;
+ afDiag[i2 + 1] = fTmp0 + fTmp2;
+ fTmp0 = fCos * fTmp1 - fTmp4;
+
+ for (int iRow = 0; iRow < 3; iRow++)
+ {
+ fTmp3 = m_aafEntry[iRow][i2 + 1];
+ m_aafEntry[iRow][i2 + 1] = fSin * m_aafEntry[iRow][i2] +
+ fCos * fTmp3;
+ m_aafEntry[iRow][i2] = fCos * m_aafEntry[iRow][i2] -
+ fSin * fTmp3;
+ }
+ }
+ afDiag[i0] -= fTmp2;
+ afSubDiag[i0] = fTmp0;
+ afSubDiag[i1] = 0.0f;
+ }
+
+ if (iIter == iMaxIter)
+ {
+ // should not get here under normal circumstances
+ return false;
+ }
+ }
+
+ return true;
+}
+//----------------------------------------------------------------------------
+void Matrix3::EigenSolveSymmetric(Real afEigenvalue[3],
+ Vector3 akEigenvector[3]) const
+{
+ Matrix3 kMatrix = *this;
+ Real afSubDiag[3];
+ kMatrix.Tridiagonal(afEigenvalue, afSubDiag);
+ kMatrix.QLAlgorithm(afEigenvalue, afSubDiag);
+
+ for (int i = 0; i < 3; i++)
+ {
+ akEigenvector[i][0] = kMatrix[0][i];
+ akEigenvector[i][1] = kMatrix[1][i];
+ akEigenvector[i][2] = kMatrix[2][i];
+ }
+
+ // make eigenvectors form a right--handed system
+ Vector3 kCross = akEigenvector[1].Cross(akEigenvector[2]);
+ Real fDet = akEigenvector[0].Dot(kCross);
+ if (fDet < 0.0f)
+ {
+ akEigenvector[2][0] = -akEigenvector[2][0];
+ akEigenvector[2][1] = -akEigenvector[2][1];
+ akEigenvector[2][2] = -akEigenvector[2][2];
+ }
+}
+//----------------------------------------------------------------------------
+void Matrix3::TensorProduct(const Vector3 &rkU, const Vector3 &rkV,
+ Matrix3 &rkProduct)
+{
+ for (int iRow = 0; iRow < 3; iRow++)
+ {
+ for (int iCol = 0; iCol < 3; iCol++)
+ rkProduct[iRow][iCol] = rkU[iRow] * rkV[iCol];
+ }
+}
+//----------------------------------------------------------------------------
diff --git a/src/editors/FreeMagic/MgcMatrix3.h b/src/editors/FreeMagic/MgcMatrix3.h
new file mode 100644
index 00000000000..9377ed6a86c
--- /dev/null
+++ b/src/editors/FreeMagic/MgcMatrix3.h
@@ -0,0 +1,166 @@
+// Magic Software, Inc.
+// http://www.magic-software.com
+// Copyright (c) 2000-2002. All Rights Reserved
+//
+// Source code from Magic Software is supplied under the terms of a license
+// agreement and may not be copied or disclosed except in accordance with the
+// terms of that agreement. The various license agreements may be found at
+// the Magic Software web site. This file is subject to the license
+//
+// FREE SOURCE CODE
+// http://www.magic-software.com/License/free.pdf
+
+#ifndef MGCMATRIX3_H
+#define MGCMATRIX3_H
+
+#include "MgcVector3.h"
+
+namespace Mgc
+{
+
+ // NOTE. The (x,y,z) coordinate system is assumed to be right-handed.
+ // Coordinate axis rotation matrices are of the form
+ // RX = 1 0 0
+ // 0 cos(t) -sin(t)
+ // 0 sin(t) cos(t)
+ // where t > 0 indicates a counterclockwise rotation in the yz-plane
+ // RY = cos(t) 0 sin(t)
+ // 0 1 0
+ // -sin(t) 0 cos(t)
+ // where t > 0 indicates a counterclockwise rotation in the zx-plane
+ // RZ = cos(t) -sin(t) 0
+ // sin(t) cos(t) 0
+ // 0 0 1
+ // where t > 0 indicates a counterclockwise rotation in the xy-plane.
+
+ class MAGICFM Matrix3
+ {
+ public:
+ // construction
+ Matrix3();
+ Matrix3(const Real aafEntry[3][3]);
+ Matrix3(const Matrix3 &rkMatrix);
+ Matrix3(Real fEntry00, Real fEntry01, Real fEntry02,
+ Real fEntry10, Real fEntry11, Real fEntry12,
+ Real fEntry20, Real fEntry21, Real fEntry22);
+
+ // member access, allows use of construct mat[r][c]
+ Real *operator[](int iRow) const;
+ operator Real *();
+ Vector3 GetColumn(int iCol) const;
+
+ // assignment and comparison
+ Matrix3 &operator=(const Matrix3 &rkMatrix);
+ bool operator==(const Matrix3 &rkMatrix) const;
+ bool operator!=(const Matrix3 &rkMatrix) const;
+
+ // arithmetic operations
+ Matrix3 operator+(const Matrix3 &rkMatrix) const;
+ Matrix3 operator-(const Matrix3 &rkMatrix) const;
+ Matrix3 operator*(const Matrix3 &rkMatrix) const;
+ Matrix3 operator-() const;
+
+ // matrix * vector [3x3 * 3x1 = 3x1]
+ Vector3 operator*(const Vector3 &rkVector) const;
+
+ // vector * matrix [1x3 * 3x3 = 1x3]
+ MAGICFM friend Vector3 operator*(const Vector3 &rkVector,
+ const Matrix3 &rkMatrix);
+
+ // matrix * scalar
+ Matrix3 operator*(Real fScalar) const;
+
+ // scalar * matrix
+ MAGICFM friend Matrix3 operator*(Real fScalar, const Matrix3 &rkMatrix);
+
+ // M0.TransposeTimes(M1) = M0^t*M1 where M0^t is the transpose of M0
+ Matrix3 TransposeTimes(const Matrix3 &rkM) const;
+
+ // M0.TimesTranspose(M1) = M0*M1^t where M1^t is the transpose of M1
+ Matrix3 TimesTranspose(const Matrix3 &rkM) const;
+
+ // utilities
+ Matrix3 Transpose() const;
+ bool Inverse(Matrix3 &rkInverse, Real fTolerance = 1e-06f) const;
+ Matrix3 Inverse(Real fTolerance = 1e-06f) const;
+ Real Determinant() const;
+
+ // SLERP (spherical linear interpolation) without quaternions. Computes
+ // R(t) = R0*(Transpose(R0)*R1)^t. If Q is a rotation matrix with
+ // unit-length axis U and angle A, then Q^t is a rotation matrix with
+ // unit-length axis U and rotation angle t*A.
+ static Matrix3 Slerp(Real fT, const Matrix3 &rkR0, const Matrix3 &rkR1);
+
+ // singular value decomposition
+ void SingularValueDecomposition(Matrix3 &rkL, Vector3 &rkS,
+ Matrix3 &rkR) const;
+ void SingularValueComposition(const Matrix3 &rkL,
+ const Vector3 &rkS, const Matrix3 &rkR);
+
+ // Gram-Schmidt orthonormalization (applied to columns of rotation matrix)
+ void Orthonormalize();
+
+ // orthogonal Q, diagonal D, upper triangular U stored as (u01,u02,u12)
+ void QDUDecomposition(Matrix3 &rkQ, Vector3 &rkD, Vector3 &rkU) const;
+
+ Real SpectralNorm() const;
+
+ // matrix must be orthonormal
+ void ToAxisAngle(Vector3 &rkAxis, Real &rfRadians) const;
+ void FromAxisAngle(const Vector3 &rkAxis, Real fRadians);
+
+ // The matrix must be orthonormal. The decomposition is yaw*pitch*roll
+ // where yaw is rotation about the Up vector, pitch is rotation about the
+ // Right axis, and roll is rotation about the Direction axis.
+ bool ToEulerAnglesXYZ(Real &rfYAngle, Real &rfPAngle,
+ Real &rfRAngle) const;
+ bool ToEulerAnglesXZY(Real &rfYAngle, Real &rfPAngle,
+ Real &rfRAngle) const;
+ bool ToEulerAnglesYXZ(Real &rfYAngle, Real &rfPAngle,
+ Real &rfRAngle) const;
+ bool ToEulerAnglesYZX(Real &rfYAngle, Real &rfPAngle,
+ Real &rfRAngle) const;
+ bool ToEulerAnglesZXY(Real &rfYAngle, Real &rfPAngle,
+ Real &rfRAngle) const;
+ bool ToEulerAnglesZYX(Real &rfYAngle, Real &rfPAngle,
+ Real &rfRAngle) const;
+ void FromEulerAnglesXYZ(Real fYAngle, Real fPAngle, Real fRAngle);
+ void FromEulerAnglesXZY(Real fYAngle, Real fPAngle, Real fRAngle);
+ void FromEulerAnglesYXZ(Real fYAngle, Real fPAngle, Real fRAngle);
+ void FromEulerAnglesYZX(Real fYAngle, Real fPAngle, Real fRAngle);
+ void FromEulerAnglesZXY(Real fYAngle, Real fPAngle, Real fRAngle);
+ void FromEulerAnglesZYX(Real fYAngle, Real fPAngle, Real fRAngle);
+
+ // eigensolver, matrix must be symmetric
+ void EigenSolveSymmetric(Real afEigenvalue[3],
+ Vector3 akEigenvector[3]) const;
+
+ static void TensorProduct(const Vector3 &rkU, const Vector3 &rkV,
+ Matrix3 &rkProduct);
+
+ static const Real EPSILON;
+ static const Matrix3 ZERO;
+ static const Matrix3 IDENTITY;
+
+ protected:
+ // support for eigensolver
+ void Tridiagonal(Real afDiag[3], Real afSubDiag[3]);
+ bool QLAlgorithm(Real afDiag[3], Real afSubDiag[3]);
+
+ // support for singular value decomposition
+ static const Real ms_fSvdEpsilon;
+ static const int ms_iSvdMaxIterations;
+ static void Bidiagonalize(Matrix3 &kA, Matrix3 &kL, Matrix3 &kR);
+ static void GolubKahanStep(Matrix3 &kA, Matrix3 &kL, Matrix3 &kR);
+
+ // support for spectral norm
+ static Real MaxCubicRoot(Real afCoeff[3]);
+
+ Real m_aafEntry[3][3];
+ };
+
+#include "MgcMatrix3.inl"
+
+} // namespace Mgc
+
+#endif
diff --git a/src/editors/FreeMagic/MgcMatrix3.inl b/src/editors/FreeMagic/MgcMatrix3.inl
new file mode 100644
index 00000000000..4a2cb7127b6
--- /dev/null
+++ b/src/editors/FreeMagic/MgcMatrix3.inl
@@ -0,0 +1,28 @@
+// Magic Software, Inc.
+// http://www.magic-software.com
+// Copyright (c) 2000-2002. All Rights Reserved
+//
+// Source code from Magic Software is supplied under the terms of a license
+// agreement and may not be copied or disclosed except in accordance with the
+// terms of that agreement. The various license agreements may be found at
+// the Magic Software web site. This file is subject to the license
+//
+// FREE SOURCE CODE
+// http://www.magic-software.com/License/free.pdf
+
+//----------------------------------------------------------------------------
+inline Matrix3::Matrix3()
+{
+ // For efficiency reasons, do not initialize matrix.
+}
+//----------------------------------------------------------------------------
+inline Real *Matrix3::operator[](int iRow) const
+{
+ return (Real *)&m_aafEntry[iRow][0];
+}
+//----------------------------------------------------------------------------
+inline Matrix3::operator Real *()
+{
+ return &m_aafEntry[0][0];
+}
+//----------------------------------------------------------------------------
diff --git a/src/editors/FreeMagic/MgcMinimize1D.cpp b/src/editors/FreeMagic/MgcMinimize1D.cpp
new file mode 100644
index 00000000000..d5001425ac0
--- /dev/null
+++ b/src/editors/FreeMagic/MgcMinimize1D.cpp
@@ -0,0 +1,277 @@
+// Magic Software, Inc.
+// http://www.magic-software.com
+// Copyright (c) 2000-2002. All Rights Reserved
+//
+// Source code from Magic Software is supplied under the terms of a license
+// agreement and may not be copied or disclosed except in accordance with the
+// terms of that agreement. The various license agreements may be found at
+// the Magic Software web site. This file is subject to the license
+//
+// FREE SOURCE CODE
+// http://www.magic-software.com/License/free.pdf
+
+#include "MgcMinimize1D.h"
+using namespace Mgc;
+
+//----------------------------------------------------------------------------
+Minimize1D::Minimize1D(Function oF, int iMaxLevel, int iMaxBracket,
+ void *pvUserData)
+{
+ assert(oF);
+ m_oF = oF;
+ m_iMaxLevel = iMaxLevel;
+ m_iMaxBracket = iMaxBracket;
+ m_pvUserData = pvUserData;
+}
+//----------------------------------------------------------------------------
+void Minimize1D::GetMinimum(Real fT0, Real fT1, Real fTInitial,
+ Real &rfTMin, Real &rfFMin)
+{
+ assert(fT0 <= fTInitial && fTInitial <= fT1);
+
+ m_fTMin = Math::MAX_REAL;
+ m_fFMin = Math::MAX_REAL;
+
+ Real fF0 = m_oF(fT0, m_pvUserData);
+ Real fFInitial = m_oF(fTInitial, m_pvUserData);
+ Real fF1 = m_oF(fT1, m_pvUserData);
+
+ GetMinimum(fT0, fF0, fTInitial, fFInitial, fT1, fF1, m_iMaxLevel);
+
+ rfTMin = m_fTMin;
+ rfFMin = m_fFMin;
+}
+//----------------------------------------------------------------------------
+void Minimize1D::GetMinimum(Real fT0, Real fF0, Real fTm, Real fFm,
+ Real fT1, Real fF1, int iLevel)
+{
+ if (fF0 < m_fFMin)
+ {
+ m_fTMin = fT0;
+ m_fFMin = fF0;
+ }
+
+ if (fFm < m_fFMin)
+ {
+ m_fTMin = fTm;
+ m_fFMin = fFm;
+ }
+
+ if (fF1 < m_fFMin)
+ {
+ m_fTMin = fT1;
+ m_fFMin = fF1;
+ }
+
+ if (iLevel-- == 0)
+ return;
+
+ if ((fT1 - fTm) * (fF0 - fFm) > (fTm - fT0) * (fFm - fF1))
+ {
+ // quadratic fit has positive second derivative at midpoint
+
+ if (fF1 > fF0)
+ {
+ if (fFm >= fF0)
+ {
+ // increasing, repeat on [t0,tm]
+ GetMinimum(fT0, fF0, fTm, fFm, iLevel);
+ }
+ else
+ {
+ // not monotonic, have a bracket
+ GetBracketedMinimum(fT0, fF0, fTm, fFm, fT1, fF1, iLevel);
+ }
+ }
+ else if (fF1 < fF0)
+ {
+ if (fFm >= fF1)
+ {
+ // decreasing, repeat on [tm,t1]
+ GetMinimum(fTm, fFm, fT1, fF1, iLevel);
+ }
+ else
+ {
+ // not monotonic, have a bracket
+ GetBracketedMinimum(fT0, fF0, fTm, fFm, fT1, fF1, iLevel);
+ }
+ }
+ else
+ {
+ // constant, repeat on [t0,tm] and [tm,t1]
+ GetMinimum(fT0, fF0, fTm, fFm, iLevel);
+ GetMinimum(fTm, fFm, fT1, fF1, iLevel);
+ }
+ }
+ else
+ {
+ // quadratic fit has nonpositive second derivative at midpoint
+
+ if (fF1 > fF0)
+ {
+ // repeat on [t0,tm]
+ GetMinimum(fT0, fF0, fTm, fFm, iLevel);
+ }
+ else if (fF1 < fF0)
+ {
+ // repeat on [tm,t1]
+ GetMinimum(fTm, fFm, fT1, fF1, iLevel);
+ }
+ else
+ {
+ // repeat on [t0,tm] and [tm,t1]
+ GetMinimum(fT0, fF0, fTm, fFm, iLevel);
+ GetMinimum(fTm, fFm, fT1, fF1, iLevel);
+ }
+ }
+}
+//----------------------------------------------------------------------------
+void Minimize1D::GetMinimum(Real fT0, Real fF0, Real fT1, Real fF1,
+ int iLevel)
+{
+ if (fF0 < m_fFMin)
+ {
+ m_fTMin = fT0;
+ m_fFMin = fF0;
+ }
+
+ if (fF1 < m_fFMin)
+ {
+ m_fTMin = fT1;
+ m_fFMin = fF1;
+ }
+
+ if (iLevel-- == 0)
+ return;
+
+ Real fTm = 0.5f * (fT0 + fT1);
+ Real fFm = m_oF(fTm, m_pvUserData);
+
+ if (fF0 - 2.0f * fFm + fF1 > 0.0f)
+ {
+ // quadratic fit has positive second derivative at midpoint
+
+ if (fF1 > fF0)
+ {
+ if (fFm >= fF0)
+ {
+ // increasing, repeat on [t0,tm]
+ GetMinimum(fT0, fF0, fTm, fFm, iLevel);
+ }
+ else
+ {
+ // not monotonic, have a bracket
+ GetBracketedMinimum(fT0, fF0, fTm, fFm, fT1, fF1, iLevel);
+ }
+ }
+ else if (fF1 < fF0)
+ {
+ if (fFm >= fF1)
+ {
+ // decreasing, repeat on [tm,t1]
+ GetMinimum(fTm, fFm, fT1, fF1, iLevel);
+ }
+ else
+ {
+ // not monotonic, have a bracket
+ GetBracketedMinimum(fT0, fF0, fTm, fFm, fT1, fF1, iLevel);
+ }
+ }
+ else
+ {
+ // constant, repeat on [t0,tm] and [tm,t1]
+ GetMinimum(fT0, fF0, fTm, fFm, iLevel);
+ GetMinimum(fTm, fFm, fT1, fF1, iLevel);
+ }
+ }
+ else
+ {
+ // quadratic fit has nonpositive second derivative at midpoint
+
+ if (fF1 > fF0)
+ {
+ // repeat on [t0,tm]
+ GetMinimum(fT0, fF0, fTm, fFm, iLevel);
+ }
+ else if (fF1 < fF0)
+ {
+ // repeat on [tm,t1]
+ GetMinimum(fTm, fFm, fT1, fF1, iLevel);
+ }
+ else
+ {
+ // repeat on [t0,tm] and [tm,t1]
+ GetMinimum(fT0, fF0, fTm, fFm, iLevel);
+ GetMinimum(fTm, fFm, fT1, fF1, iLevel);
+ }
+ }
+}
+//----------------------------------------------------------------------------
+void Minimize1D::GetBracketedMinimum(Real fT0, Real fF0, Real fTm,
+ Real fFm, Real fT1, Real fF1, int iLevel)
+{
+ for (int i = 0; i < m_iMaxBracket; i++)
+ {
+ // update minimum value
+ if (fFm < m_fFMin)
+ {
+ m_fTMin = fTm;
+ m_fFMin = fFm;
+ }
+
+ // test for convergence
+ const Real fEps = 1e-08f, fTol = 1e-04f;
+ if (Math::FAbs(fT1 - fT0) <= 2.0f * fTol * Math::FAbs(fTm) + fEps)
+ break;
+
+ // compute vertex of interpolating parabola
+ Real fDT0 = fT0 - fTm, fDT1 = fT1 - fTm;
+ Real fDF0 = fF0 - fFm, fDF1 = fF1 - fFm;
+ Real fTmp0 = fDT0 * fDF1, fTmp1 = fDT1 * fDF0;
+ Real fDenom = fTmp1 - fTmp0;
+ if (Math::FAbs(fDenom) < fEps)
+ return;
+
+ Real fTv = fTm + 0.5f * (fDT1 * fTmp1 - fDT0 * fTmp0) / fDenom;
+ assert(fT0 <= fTv && fTv <= fT1);
+ Real fFv = m_oF(fTv, m_pvUserData);
+
+ if (fTv < fTm)
+ {
+ if (fFv < fFm)
+ {
+ fT1 = fTm;
+ fF1 = fFm;
+ fTm = fTv;
+ fFm = fFv;
+ }
+ else
+ {
+ fT0 = fTv;
+ fF0 = fFv;
+ }
+ }
+ else if (fTv > fTm)
+ {
+ if (fFv < fFm)
+ {
+ fT0 = fTm;
+ fF0 = fFm;
+ fTm = fTv;
+ fFm = fFv;
+ }
+ else
+ {
+ fT1 = fTv;
+ fF1 = fFv;
+ }
+ }
+ else
+ {
+ // vertex of parabola is already at middle sample point
+ GetMinimum(fT0, fF0, fTm, fFm, iLevel);
+ GetMinimum(fTm, fFm, fT1, fF1, iLevel);
+ }
+ }
+}
+//----------------------------------------------------------------------------
diff --git a/src/editors/FreeMagic/MgcMinimize1D.h b/src/editors/FreeMagic/MgcMinimize1D.h
new file mode 100644
index 00000000000..167a9387784
--- /dev/null
+++ b/src/editors/FreeMagic/MgcMinimize1D.h
@@ -0,0 +1,55 @@
+// Magic Software, Inc.
+// http://www.magic-software.com
+// Copyright (c) 2000-2002. All Rights Reserved
+//
+// Source code from Magic Software is supplied under the terms of a license
+// agreement and may not be copied or disclosed except in accordance with the
+// terms of that agreement. The various license agreements may be found at
+// the Magic Software web site. This file is subject to the license
+//
+// FREE SOURCE CODE
+// http://www.magic-software.com/License/free.pdf
+
+#ifndef MGCMINIMIZE1D_H
+#define MGCMINIMIZE1D_H
+
+#include "MgcMath.h"
+
+namespace Mgc
+{
+
+ class MAGICFM Minimize1D
+ {
+ public:
+ typedef Real (*Function)(Real, void *);
+
+ Minimize1D(Function oF, int iMaxLevel, int iMaxBracket,
+ void *pvUserData = 0);
+
+ int &MaxLevel();
+ int &MaxBracket();
+ void *&UserData();
+
+ void GetMinimum(Real fT0, Real fT1, Real fTInitial,
+ Real &rfTMin, Real &rfFMin);
+
+ protected:
+ Function m_oF;
+ int m_iMaxLevel, m_iMaxBracket;
+ Real m_fTMin, m_fFMin;
+ void *m_pvUserData;
+
+ void GetMinimum(Real fT0, Real fF0, Real fT1, Real fF1, int iLevel);
+
+ void GetMinimum(Real fT0, Real fF0, Real fTm, Real fFm, Real fT1,
+ Real fF1, int iLevel);
+
+ void GetBracketedMinimum(Real fT0, Real fF0, Real fTm,
+ Real fFm, Real fT1, Real fF1, int iLevel);
+ };
+
+#include "MgcMinimize1D.inl"
+
+} // namespace Mgc
+
+#endif
diff --git a/src/editors/FreeMagic/MgcMinimize1D.inl b/src/editors/FreeMagic/MgcMinimize1D.inl
new file mode 100644
index 00000000000..34c7263240c
--- /dev/null
+++ b/src/editors/FreeMagic/MgcMinimize1D.inl
@@ -0,0 +1,28 @@
+// Magic Software, Inc.
+// http://www.magic-software.com
+// Copyright (c) 2000-2002. All Rights Reserved
+//
+// Source code from Magic Software is supplied under the terms of a license
+// agreement and may not be copied or disclosed except in accordance with the
+// terms of that agreement. The various license agreements may be found at
+// the Magic Software web site. This file is subject to the license
+//
+// FREE SOURCE CODE
+// http://www.magic-software.com/License/free.pdf
+
+//----------------------------------------------------------------------------
+inline int &Minimize1D::MaxLevel()
+{
+ return m_iMaxLevel;
+}
+//----------------------------------------------------------------------------
+inline int &Minimize1D::MaxBracket()
+{
+ return m_iMaxBracket;
+}
+//----------------------------------------------------------------------------
+inline void *&Minimize1D::UserData()
+{
+ return m_pvUserData;
+}
+//----------------------------------------------------------------------------
diff --git a/src/editors/FreeMagic/MgcMinimizeND.cpp b/src/editors/FreeMagic/MgcMinimizeND.cpp
new file mode 100644
index 00000000000..0f38020da6b
--- /dev/null
+++ b/src/editors/FreeMagic/MgcMinimizeND.cpp
@@ -0,0 +1,175 @@
+// Magic Software, Inc.
+// http://www.magic-software.com
+// Copyright (c) 2000-2002. All Rights Reserved
+//
+// Source code from Magic Software is supplied under the terms of a license
+// agreement and may not be copied or disclosed except in accordance with the
+// terms of that agreement. The various license agreements may be found at
+// the Magic Software web site. This file is subject to the license
+//
+// FREE SOURCE CODE
+// http://www.magic-software.com/License/free.pdf
+
+#include "MgcMinimizeND.h"
+using namespace Mgc;
+
+//----------------------------------------------------------------------------
+MinimizeND::MinimizeND(int iDimensions, Function oF, int iMaxLevel,
+ int iMaxBracket, int iMaxIterations, void *pvUserData)
+ : m_kMinimizer(LineFunction, iMaxLevel, iMaxBracket)
+{
+ assert(iDimensions >= 1 && oF);
+
+ m_iDimensions = iDimensions;
+ m_oF = oF;
+ m_iMaxIterations = iMaxIterations;
+ m_pvUserData = pvUserData;
+
+ m_afTCurr = new Real[m_iDimensions];
+ m_afTSave = new Real[m_iDimensions];
+ m_afDirectionStorage = new Real[m_iDimensions * (m_iDimensions + 1)];
+ m_aafDirection = new Real *[m_iDimensions + 1];
+ for (int i = 0; i <= m_iDimensions; i++)
+ m_aafDirection[i] = &m_afDirectionStorage[i * m_iDimensions];
+ m_afDConj = m_aafDirection[m_iDimensions];
+
+ m_afLineArg = new Real[m_iDimensions];
+}
+//----------------------------------------------------------------------------
+MinimizeND::~MinimizeND()
+{
+ delete[] m_afTCurr;
+ delete[] m_afTSave;
+ delete[] m_afDirectionStorage;
+ delete[] m_aafDirection;
+ delete[] m_afLineArg;
+}
+//----------------------------------------------------------------------------
+void MinimizeND::GetMinimum(const Real *afT0, const Real *afT1,
+ const Real *afTInitial, Real *afTMin, Real &rfFMin)
+{
+ // for 1D function callback
+ m_kMinimizer.UserData() = this;
+
+ // initial guess
+ int iQuantity = m_iDimensions * sizeof(Real);
+ m_fFCurr = m_oF(afTInitial, m_pvUserData);
+ memcpy(m_afTSave, afTInitial, iQuantity);
+ memcpy(m_afTCurr, afTInitial, iQuantity);
+
+ // initialize direction set to the standard Euclidean basis
+ memset(m_afDirectionStorage, 0, iQuantity * (m_iDimensions + 1));
+ int i;
+ for (i = 0; i < m_iDimensions; i++)
+ m_aafDirection[i][i] = 1.0f;
+
+ Real fL0, fL1, fLMin;
+ for (int iIter = 0; iIter < m_iMaxIterations; iIter++)
+ {
+ // find minimum in each direction and update current location
+ for (i = 0; i < m_iDimensions; i++)
+ {
+ m_afDCurr = m_aafDirection[i];
+ ComputeDomain(afT0, afT1, fL0, fL1);
+ m_kMinimizer.GetMinimum(fL0, fL1, 0.0f, fLMin, m_fFCurr);
+ for (int j = 0; j < m_iDimensions; j++)
+ m_afTCurr[j] += fLMin * m_afDCurr[j];
+ }
+
+ // estimate a unit-length conjugate direction
+ Real fLength = 0.0f;
+ for (i = 0; i < m_iDimensions; i++)
+ {
+ m_afDConj[i] = m_afTCurr[i] - m_afTSave[i];
+ fLength += m_afDConj[i] * m_afDConj[i];
+ }
+
+ const Real fEpsilon = 1e-06f;
+ fLength = Math::Sqrt(fLength);
+ if (fLength < fEpsilon)
+ {
+ // New position did not change significantly from old one.
+ // Should there be a better convergence criterion here?
+ break;
+ }
+
+ Real fInvLength = 1.0f / fLength;
+ for (i = 0; i < m_iDimensions; i++)
+ m_afDConj[i] *= fInvLength;
+
+ // minimize in conjugate direction
+ m_afDCurr = m_afDConj;
+ ComputeDomain(afT0, afT1, fL0, fL1);
+ m_kMinimizer.GetMinimum(fL0, fL1, 0.0f, fLMin, m_fFCurr);
+ for (i = 0; i < m_iDimensions; i++)
+ m_afTCurr[i] += fLMin * m_afDCurr[i];
+
+ // cycle the directions and add conjugate direction to set
+ m_afDConj = m_aafDirection[0];
+ for (i = 0; i < m_iDimensions; i++)
+ m_aafDirection[i] = m_aafDirection[i + 1];
+
+ // set parameters for next pass
+ memcpy(m_afTSave, m_afTCurr, iQuantity);
+ }
+
+ memcpy(afTMin, m_afTCurr, iQuantity);
+ rfFMin = m_fFCurr;
+}
+//----------------------------------------------------------------------------
+void MinimizeND::ComputeDomain(const Real *afT0, const Real *afT1,
+ Real &rfL0, Real &rfL1)
+{
+ rfL0 = -Math::MAX_REAL;
+ rfL1 = +Math::MAX_REAL;
+
+ for (int i = 0; i < m_iDimensions; i++)
+ {
+ Real fB0 = afT0[i] - m_afTCurr[i];
+ Real fB1 = afT1[i] - m_afTCurr[i];
+ Real fInv;
+ if (m_afDCurr[i] > 0.0f)
+ {
+ // valid t-interval is [B0,B1]
+ fInv = 1.0f / m_afDCurr[i];
+ fB0 *= fInv;
+ if (fB0 > rfL0)
+ rfL0 = fB0;
+ fB1 *= fInv;
+ if (fB1 < rfL1)
+ rfL1 = fB1;
+ }
+ else if (m_afDCurr[i] < 0.0f)
+ {
+ // valid t-interval is [B1,B0]
+ fInv = 1.0f / m_afDCurr[i];
+ fB0 *= fInv;
+ if (fB0 < rfL1)
+ rfL1 = fB0;
+ fB1 *= fInv;
+ if (fB1 > rfL0)
+ rfL0 = fB1;
+ }
+ }
+
+ // correction if numerical errors lead to values nearly zero
+ if (rfL0 > 0.0f)
+ rfL0 = 0.0f;
+ if (rfL1 < 0.0f)
+ rfL1 = 0.0f;
+}
+//----------------------------------------------------------------------------
+Real MinimizeND::LineFunction(Real fT, void *pvUserData)
+{
+ MinimizeND *pkThis = (MinimizeND *)pvUserData;
+
+ for (int i = 0; i < pkThis->m_iDimensions; i++)
+ {
+ pkThis->m_afLineArg[i] = pkThis->m_afTCurr[i] +
+ fT * pkThis->m_afDCurr[i];
+ }
+
+ Real fResult = pkThis->m_oF(pkThis->m_afLineArg, pkThis->m_pvUserData);
+ return fResult;
+}
+//----------------------------------------------------------------------------
diff --git a/src/editors/FreeMagic/MgcMinimizeND.h b/src/editors/FreeMagic/MgcMinimizeND.h
new file mode 100644
index 00000000000..24e81f4896f
--- /dev/null
+++ b/src/editors/FreeMagic/MgcMinimizeND.h
@@ -0,0 +1,64 @@
+// Magic Software, Inc.
+// http://www.magic-software.com
+// Copyright (c) 2000-2002. All Rights Reserved
+//
+// Source code from Magic Software is supplied under the terms of a license
+// agreement and may not be copied or disclosed except in accordance with the
+// terms of that agreement. The various license agreements may be found at
+// the Magic Software web site. This file is subject to the license
+//
+// FREE SOURCE CODE
+// http://www.magic-software.com/License/free.pdf
+
+#ifndef MGCMINIMIZEND_H
+#define MGCMINIMIZEND_H
+
+#include "MgcMinimize1D.h"
+
+namespace Mgc
+{
+
+ class MAGICFM MinimizeND
+ {
+ public:
+ typedef Real (*Function)(const Real *, void *);
+
+ MinimizeND(int iDimensions, Function oF, int iMaxLevel, int iMaxBracket,
+ int iMaxIterations, void *pvUserData = 0);
+
+ ~MinimizeND();
+
+ int &MaxLevel();
+ int &MaxBracket();
+ void *&UserData();
+
+ // find minimum on Cartesian-product domain
+ void GetMinimum(const Real *afT0, const Real *afT1,
+ const Real *afTInitial, Real *afTMin, Real &rfFMin);
+
+ protected:
+ int m_iDimensions;
+ Function m_oF;
+ int m_iMaxIterations;
+ void *m_pvUserData;
+ Minimize1D m_kMinimizer;
+ Real *m_afDirectionStorage;
+ Real **m_aafDirection;
+ Real *m_afDConj;
+ Real *m_afDCurr;
+ Real *m_afTSave;
+ Real *m_afTCurr;
+ Real m_fFCurr;
+ Real *m_afLineArg;
+
+ void ComputeDomain(const Real *afT0, const Real *afT1,
+ Real &rfL0, Real &rfL1);
+
+ static Real LineFunction(Real fT, void *pvUserData);
+ };
+
+#include "MgcMinimizeND.inl"
+
+} // namespace Mgc
+
+#endif
diff --git a/src/editors/FreeMagic/MgcMinimizeND.inl b/src/editors/FreeMagic/MgcMinimizeND.inl
new file mode 100644
index 00000000000..0774f8b2776
--- /dev/null
+++ b/src/editors/FreeMagic/MgcMinimizeND.inl
@@ -0,0 +1,28 @@
+// Magic Software, Inc.
+// http://www.magic-software.com
+// Copyright (c) 2000-2002. All Rights Reserved
+//
+// Source code from Magic Software is supplied under the terms of a license
+// agreement and may not be copied or disclosed except in accordance with the
+// terms of that agreement. The various license agreements may be found at
+// the Magic Software web site. This file is subject to the license
+//
+// FREE SOURCE CODE
+// http://www.magic-software.com/License/free.pdf
+
+//----------------------------------------------------------------------------
+inline int &MinimizeND::MaxLevel()
+{
+ return m_kMinimizer.MaxLevel();
+}
+//----------------------------------------------------------------------------
+inline int &MinimizeND::MaxBracket()
+{
+ return m_kMinimizer.MaxBracket();
+}
+//----------------------------------------------------------------------------
+inline void *&MinimizeND::UserData()
+{
+ return m_pvUserData;
+}
+//----------------------------------------------------------------------------
diff --git a/src/editors/FreeMagic/MgcQuaternion.cpp b/src/editors/FreeMagic/MgcQuaternion.cpp
new file mode 100644
index 00000000000..52a463bea37
--- /dev/null
+++ b/src/editors/FreeMagic/MgcQuaternion.cpp
@@ -0,0 +1,390 @@
+// Magic Software, Inc.
+// http://www.magic-software.com
+// Copyright (c) 2000-2002. All Rights Reserved
+//
+// Source code from Magic Software is supplied under the terms of a license
+// agreement and may not be copied or disclosed except in accordance with the
+// terms of that agreement. The various license agreements may be found at
+// the Magic Software web site. This file is subject to the license
+//
+// FREE SOURCE CODE
+// http://www.magic-software.com/License/free.pdf
+#pragma hdrstop
+#include "MgcQuaternion.h"
+using namespace Mgc;
+
+static Real gs_fEpsilon = 1e-03f;
+Quaternion Quaternion::ZERO(0.0f, 0.0f, 0.0f, 0.0f);
+Quaternion Quaternion::IDENTITY(1.0f, 0.0f, 0.0f, 0.0f);
+
+//----------------------------------------------------------------------------
+Quaternion::Quaternion(Real fW, Real fX, Real fY, Real fZ)
+{
+ w = fW;
+ x = fX;
+ y = fY;
+ z = fZ;
+}
+//----------------------------------------------------------------------------
+Quaternion::Quaternion(const Quaternion &rkQ)
+{
+ w = rkQ.w;
+ x = rkQ.x;
+ y = rkQ.y;
+ z = rkQ.z;
+}
+//----------------------------------------------------------------------------
+void Quaternion::FromRotationMatrix(const Matrix3 &kRot)
+{
+ // Algorithm in Ken Shoemake's article in 1987 SIGGRAPH course notes
+ // article "Quaternion Calculus and Fast Animation".
+
+ Real fTrace = kRot[0][0] + kRot[1][1] + kRot[2][2];
+ Real fRoot;
+
+ if (fTrace > 0.0f)
+ {
+ // |w| > 1/2, may as well choose w > 1/2
+ fRoot = Math::Sqrt(fTrace + 1.0f); // 2w
+ w = 0.5f * fRoot;
+ fRoot = 0.5f / fRoot; // 1/(4w)
+ x = (kRot[2][1] - kRot[1][2]) * fRoot;
+ y = (kRot[0][2] - kRot[2][0]) * fRoot;
+ z = (kRot[1][0] - kRot[0][1]) * fRoot;
+ }
+ else
+ {
+ // |w| <= 1/2
+ static int s_iNext[3] = {1, 2, 0};
+ int i = 0;
+ if (kRot[1][1] > kRot[0][0])
+ i = 1;
+ if (kRot[2][2] > kRot[i][i])
+ i = 2;
+ int j = s_iNext[i];
+ int k = s_iNext[j];
+
+ fRoot = Math::Sqrt(kRot[i][i] - kRot[j][j] - kRot[k][k] + 1.0f);
+ Real *apkQuat[3] = {&x, &y, &z};
+ *apkQuat[i] = 0.5f * fRoot;
+ fRoot = 0.5f / fRoot;
+ w = (kRot[k][j] - kRot[j][k]) * fRoot;
+ *apkQuat[j] = (kRot[j][i] + kRot[i][j]) * fRoot;
+ *apkQuat[k] = (kRot[k][i] + kRot[i][k]) * fRoot;
+ }
+}
+//----------------------------------------------------------------------------
+void Quaternion::ToRotationMatrix(Matrix3 &kRot) const
+{
+ Real fTx = 2.0f * x;
+ Real fTy = 2.0f * y;
+ Real fTz = 2.0f * z;
+ Real fTwx = fTx * w;
+ Real fTwy = fTy * w;
+ Real fTwz = fTz * w;
+ Real fTxx = fTx * x;
+ Real fTxy = fTy * x;
+ Real fTxz = fTz * x;
+ Real fTyy = fTy * y;
+ Real fTyz = fTz * y;
+ Real fTzz = fTz * z;
+
+ kRot[0][0] = 1.0f - (fTyy + fTzz);
+ kRot[0][1] = fTxy - fTwz;
+ kRot[0][2] = fTxz + fTwy;
+ kRot[1][0] = fTxy + fTwz;
+ kRot[1][1] = 1.0f - (fTxx + fTzz);
+ kRot[1][2] = fTyz - fTwx;
+ kRot[2][0] = fTxz - fTwy;
+ kRot[2][1] = fTyz + fTwx;
+ kRot[2][2] = 1.0f - (fTxx + fTyy);
+}
+//----------------------------------------------------------------------------
+void Quaternion::FromAngleAxis(const Real &rfAngle, const Vector3 &rkAxis)
+{
+ // 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 = 0.5f * rfAngle;
+ Real fSin = Math::Sin(fHalfAngle);
+ w = Math::Cos(fHalfAngle);
+ x = fSin * rkAxis.x;
+ y = fSin * rkAxis.y;
+ z = fSin * rkAxis.z;
+}
+//----------------------------------------------------------------------------
+void Quaternion::ToAngleAxis(Real &rfAngle, Vector3 &rkAxis) const
+{
+ // The quaternion representing the rotation is
+ // q = cos(A/2)+sin(A/2)*(x*i+y*j+z*k)
+
+ Real fSqrLength = x * x + y * y + z * z;
+ if (fSqrLength > 0.0f)
+ {
+ rfAngle = 2.0f * Math::ACos(w);
+ Real fInvLength = Math::InvSqrt(fSqrLength);
+ rkAxis.x = x * fInvLength;
+ rkAxis.y = y * fInvLength;
+ rkAxis.z = z * fInvLength;
+ }
+ else
+ {
+ // angle is 0 (mod 2*pi), so any axis will do
+ rfAngle = 0.0f;
+ rkAxis.x = 1.0f;
+ rkAxis.y = 0.0f;
+ rkAxis.z = 0.0f;
+ }
+}
+//----------------------------------------------------------------------------
+void Quaternion::FromAxes(const Vector3 *akAxis)
+{
+ Matrix3 kRot;
+
+ for (int iCol = 0; iCol < 3; iCol++)
+ {
+ kRot[0][iCol] = akAxis[iCol].x;
+ kRot[1][iCol] = akAxis[iCol].y;
+ kRot[2][iCol] = akAxis[iCol].z;
+ }
+
+ FromRotationMatrix(kRot);
+}
+//----------------------------------------------------------------------------
+void Quaternion::ToAxes(Vector3 *akAxis) const
+{
+ Matrix3 kRot;
+
+ ToRotationMatrix(kRot);
+
+ for (int iCol = 0; iCol < 3; iCol++)
+ {
+ akAxis[iCol].x = kRot[0][iCol];
+ akAxis[iCol].y = kRot[1][iCol];
+ akAxis[iCol].z = kRot[2][iCol];
+ }
+}
+//----------------------------------------------------------------------------
+Quaternion &Quaternion::operator=(const Quaternion &rkQ)
+{
+ w = rkQ.w;
+ x = rkQ.x;
+ y = rkQ.y;
+ z = rkQ.z;
+ return *this;
+}
+//----------------------------------------------------------------------------
+Quaternion Quaternion::operator+(const Quaternion &rkQ) const
+{
+ return Quaternion(w + rkQ.w, x + rkQ.x, y + rkQ.y, z + rkQ.z);
+}
+//----------------------------------------------------------------------------
+Quaternion Quaternion::operator-(const Quaternion &rkQ) const
+{
+ return Quaternion(w - rkQ.w, x - rkQ.x, y - rkQ.y, z - rkQ.z);
+}
+//----------------------------------------------------------------------------
+Quaternion Quaternion::operator*(const Quaternion &rkQ) const
+{
+ // NOTE: Multiplication is not generally commutative, so in most
+ // cases p*q != q*p.
+
+ return Quaternion(
+ w * rkQ.w - x * rkQ.x - y * rkQ.y - z * rkQ.z,
+ w * rkQ.x + x * rkQ.w + y * rkQ.z - z * rkQ.y,
+ w * rkQ.y + y * rkQ.w + z * rkQ.x - x * rkQ.z,
+ w * rkQ.z + z * rkQ.w + x * rkQ.y - y * rkQ.x);
+}
+//----------------------------------------------------------------------------
+Quaternion Quaternion::operator*(Real fScalar) const
+{
+ return Quaternion(fScalar * w, fScalar * x, fScalar * y, fScalar * z);
+}
+//----------------------------------------------------------------------------
+Quaternion Mgc::operator*(Real fScalar, const Quaternion &rkQ)
+{
+ return Quaternion(fScalar * rkQ.w, fScalar * rkQ.x, fScalar * rkQ.y,
+ fScalar * rkQ.z);
+}
+//----------------------------------------------------------------------------
+Quaternion Quaternion::operator-() const
+{
+ return Quaternion(-w, -x, -y, -z);
+}
+//----------------------------------------------------------------------------
+Real Quaternion::Dot(const Quaternion &rkQ) const
+{
+ return w * rkQ.w + x * rkQ.x + y * rkQ.y + z * rkQ.z;
+}
+//----------------------------------------------------------------------------
+Real Quaternion::Norm() const
+{
+ return w * w + x * x + y * y + z * z;
+}
+//----------------------------------------------------------------------------
+Quaternion Quaternion::Inverse() const
+{
+ Real fNorm = w * w + x * x + y * y + z * z;
+ if (fNorm > 0.0f)
+ {
+ Real fInvNorm = 1.0f / fNorm;
+ return Quaternion(w * fInvNorm, -x * fInvNorm, -y * fInvNorm, -z * fInvNorm);
+ }
+ else
+ {
+ // return an invalid result to flag the error
+ return ZERO;
+ }
+}
+//----------------------------------------------------------------------------
+Quaternion Quaternion::UnitInverse() const
+{
+ // assert: 'this' is unit length
+ return Quaternion(w, -x, -y, -z);
+}
+//----------------------------------------------------------------------------
+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.
+
+ Real fAngle = Math::Sqrt(x * x + y * y + z * z);
+ Real fSin = Math::Sin(fAngle);
+
+ Quaternion kResult;
+ kResult.w = Math::Cos(fAngle);
+
+ if (Math::FAbs(fSin) >= gs_fEpsilon)
+ {
+ Real fCoeff = fSin / fAngle;
+ kResult.x = fCoeff * x;
+ kResult.y = fCoeff * y;
+ kResult.z = fCoeff * z;
+ }
+ else
+ {
+ kResult.x = x;
+ kResult.y = y;
+ kResult.z = z;
+ }
+
+ return kResult;
+}
+//----------------------------------------------------------------------------
+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.w = 0.0f;
+
+ if (Math::FAbs(w) < 1.0f)
+ {
+ Real fAngle = Math::ACos(w);
+ Real fSin = Math::Sin(fAngle);
+ if (Math::FAbs(fSin) >= gs_fEpsilon)
+ {
+ Real fCoeff = fAngle / fSin;
+ kResult.x = fCoeff * x;
+ kResult.y = fCoeff * y;
+ kResult.z = fCoeff * z;
+ return kResult;
+ }
+ }
+
+ kResult.x = x;
+ kResult.y = y;
+ kResult.z = z;
+
+ return kResult;
+}
+//----------------------------------------------------------------------------
+Vector3 Quaternion::operator*(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;
+}
+//----------------------------------------------------------------------------
+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) < gs_fEpsilon)
+ return rkP;
+
+ Real fSin = Math::Sin(fAngle);
+ Real fInvSin = 1.0f / fSin;
+ Real fCoeff0 = Math::Sin((1.0f - fT) * fAngle) * fInvSin;
+ Real fCoeff1 = Math::Sin(fT * fAngle) * fInvSin;
+ return fCoeff0 * rkP + fCoeff1 * rkQ;
+}
+//----------------------------------------------------------------------------
+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) < gs_fEpsilon)
+ return rkP;
+
+ Real fSin = Math::Sin(fAngle);
+ Real fPhase = Math::_PI * iExtraSpins * fT;
+ Real fInvSin = 1.0f / fSin;
+ Real fCoeff0 = Math::Sin((1.0f - fT) * fAngle - fPhase) * fInvSin;
+ Real fCoeff1 = Math::Sin(fT * fAngle + fPhase) * fInvSin;
+ return fCoeff0 * rkP + fCoeff1 * rkQ;
+}
+//----------------------------------------------------------------------------
+void Quaternion::Intermediate(const Quaternion &rkQ0,
+ const Quaternion &rkQ1, const Quaternion &rkQ2, Quaternion &rkA,
+ Quaternion &rkB)
+{
+ // assert: q0, q1, q2 are unit quaternions
+
+ Quaternion kQ0inv = rkQ0.UnitInverse();
+ Quaternion kQ1inv = rkQ1.UnitInverse();
+ Quaternion rkP0 = kQ0inv * rkQ1;
+ Quaternion rkP1 = kQ1inv * rkQ2;
+ Quaternion kArg = 0.25f * (rkP0.Log() - rkP1.Log());
+ Quaternion kMinusArg = -kArg;
+
+ rkA = rkQ1 * kArg.Exp();
+ rkB = rkQ1 * kMinusArg.Exp();
+}
+//----------------------------------------------------------------------------
+Quaternion Quaternion::Squad(Real fT, const Quaternion &rkP,
+ const Quaternion &rkA, const Quaternion &rkB, const Quaternion &rkQ)
+{
+ Real fSlerpT = 2.0f * fT * (1.0f - fT);
+ Quaternion kSlerpP = Slerp(fT, rkP, rkQ);
+ Quaternion kSlerpQ = Slerp(fT, rkA, rkB);
+ return Slerp(fSlerpT, kSlerpP, kSlerpQ);
+}
+//----------------------------------------------------------------------------
diff --git a/src/editors/FreeMagic/MgcQuaternion.h b/src/editors/FreeMagic/MgcQuaternion.h
new file mode 100644
index 00000000000..6f44a48b4eb
--- /dev/null
+++ b/src/editors/FreeMagic/MgcQuaternion.h
@@ -0,0 +1,85 @@
+// Magic Software, Inc.
+// http://www.magic-software.com
+// Copyright (c) 2000-2002. All Rights Reserved
+//
+// Source code from Magic Software is supplied under the terms of a license
+// agreement and may not be copied or disclosed except in accordance with the
+// terms of that agreement. The various license agreements may be found at
+// the Magic Software web site. This file is subject to the license
+//
+// FREE SOURCE CODE
+// http://www.magic-software.com/License/free.pdf
+
+#ifndef MGCQUATERNION_H
+#define MGCQUATERNION_H
+
+#include "MgcMatrix3.h"
+
+namespace Mgc
+{
+
+ class MAGICFM Quaternion
+ {
+ public:
+ Real w, x, y, z;
+
+ // construction and destruction
+ Quaternion(Real fW = 1.0f, Real fX = 0.0f, Real fY = 0.0f,
+ Real fZ = 0.0f);
+ Quaternion(const Quaternion &rkQ);
+
+ // conversion between quaternions, matrices, and angle-axes
+ void FromRotationMatrix(const Matrix3 &kRot);
+ void ToRotationMatrix(Matrix3 &kRot) const;
+ void FromAngleAxis(const Real &rfAngle, const Vector3 &rkAxis);
+ void ToAngleAxis(Real &rfAngle, Vector3 &rkAxis) const;
+ void FromAxes(const Vector3 *akAxis);
+ void ToAxes(Vector3 *akAxis) const;
+
+ // arithmetic operations
+ Quaternion &operator=(const Quaternion &rkQ);
+ Quaternion operator+(const Quaternion &rkQ) const;
+ Quaternion operator-(const Quaternion &rkQ) const;
+ Quaternion operator*(const Quaternion &rkQ) const;
+ Quaternion operator*(Real fScalar) const;
+ MAGICFM friend Quaternion operator*(Real fScalar,
+ const Quaternion &rkQ);
+ Quaternion operator-() const;
+
+ // functions of a quaternion
+ Real Dot(const Quaternion &rkQ) const; // dot product
+ Real Norm() const; // squared-length
+ Quaternion Inverse() const; // apply to non-zero quaternion
+ Quaternion UnitInverse() const; // apply to unit-length quaternion
+ Quaternion Exp() const;
+ Quaternion Log() const;
+
+ // rotation of a vector by a quaternion
+ Vector3 operator*(const Vector3 &rkVector) const;
+
+ // spherical linear interpolation
+ static Quaternion Slerp(Real fT, const Quaternion &rkP,
+ const Quaternion &rkQ);
+
+ static Quaternion SlerpExtraSpins(Real fT,
+ const Quaternion &rkP, const Quaternion &rkQ,
+ int iExtraSpins);
+
+ // setup for spherical quadratic interpolation
+ static void Intermediate(const Quaternion &rkQ0,
+ const Quaternion &rkQ1, const Quaternion &rkQ2,
+ Quaternion &rka, Quaternion &rkB);
+
+ // spherical quadratic interpolation
+ static Quaternion Squad(Real fT, const Quaternion &rkP,
+ const Quaternion &rkA, const Quaternion &rkB,
+ const Quaternion &rkQ);
+
+ // special values
+ static Quaternion ZERO;
+ static Quaternion IDENTITY;
+ };
+
+} // namespace Mgc
+
+#endif
diff --git a/src/editors/FreeMagic/MgcRTLib.h b/src/editors/FreeMagic/MgcRTLib.h
new file mode 100644
index 00000000000..b1bf3956142
--- /dev/null
+++ b/src/editors/FreeMagic/MgcRTLib.h
@@ -0,0 +1,34 @@
+// Magic Software, Inc.
+// http://www.magic-software.com
+// Copyright (c) 2000-2002. All Rights Reserved
+//
+// Source code from Magic Software is supplied under the terms of a license
+// agreement and may not be copied or disclosed except in accordance with the
+// terms of that agreement. The various license agreements may be found at
+// the Magic Software web site. This file is subject to the license
+//
+// FREE SOURCE CODE
+// http://www.magic-software.com/License/free.pdf
+
+#ifndef MGCRTLIB_H
+#define MGCRTLIB_H
+
+// A wrapper around some headers for run-time libraries. I added this because
+// CodeWarrior 6.1 for the Macintosh uses namespace std for functions exposed
+// in cmath, cstring, etc. Windows or Linux does not do this. I do not know
+// what the STL standard is for this.
+
+#include
+#include
+#include
+#include
+#include
+#include
+#include
+#include
+
+#if defined(__MACOS__) && !defined(__MACH__)
+using namespace std;
+#endif
+
+#endif
diff --git a/src/editors/FreeMagic/MgcRay3.h b/src/editors/FreeMagic/MgcRay3.h
new file mode 100644
index 00000000000..8d214c75a23
--- /dev/null
+++ b/src/editors/FreeMagic/MgcRay3.h
@@ -0,0 +1,42 @@
+// Magic Software, Inc.
+// http://www.magic-software.com
+// Copyright (c) 2000-2002. All Rights Reserved
+//
+// Source code from Magic Software is supplied under the terms of a license
+// agreement and may not be copied or disclosed except in accordance with the
+// terms of that agreement. The various license agreements may be found at
+// the Magic Software web site. This file is subject to the license
+//
+// FREE SOURCE CODE
+// http://www.magic-software.com/License/free.pdf
+
+#ifndef MGCRAY3_H
+#define MGCRAY3_H
+
+#include "MgcVector3.h"
+
+namespace Mgc
+{
+
+ class MAGICFM Ray3
+ {
+ public:
+ // Ray is R(t) = P+t*D for t >= 0. D is not necessarily unit length.
+ Ray3();
+
+ Vector3 &Origin();
+ const Vector3 &Origin() const;
+
+ Vector3 &Direction();
+ const Vector3 &Direction() const;
+
+ protected:
+ Vector3 m_kOrigin; // P
+ Vector3 m_kDirection; // D
+ };
+
+#include "MgcRay3.inl"
+
+} // namespace Mgc
+
+#endif
diff --git a/src/editors/FreeMagic/MgcRay3.inl b/src/editors/FreeMagic/MgcRay3.inl
new file mode 100644
index 00000000000..73de6bed9d2
--- /dev/null
+++ b/src/editors/FreeMagic/MgcRay3.inl
@@ -0,0 +1,38 @@
+// Magic Software, Inc.
+// http://www.magic-software.com
+// Copyright (c) 2000-2002. All Rights Reserved
+//
+// Source code from Magic Software is supplied under the terms of a license
+// agreement and may not be copied or disclosed except in accordance with the
+// terms of that agreement. The various license agreements may be found at
+// the Magic Software web site. This file is subject to the license
+//
+// FREE SOURCE CODE
+// http://www.magic-software.com/License/free.pdf
+
+//----------------------------------------------------------------------------
+inline Ray3::Ray3()
+{
+ // no initialization for efficiency
+}
+//----------------------------------------------------------------------------
+inline Vector3 &Ray3::Origin()
+{
+ return m_kOrigin;
+}
+//----------------------------------------------------------------------------
+inline const Vector3 &Ray3::Origin() const
+{
+ return m_kOrigin;
+}
+//----------------------------------------------------------------------------
+inline Vector3 &Ray3::Direction()
+{
+ return m_kDirection;
+}
+//----------------------------------------------------------------------------
+inline const Vector3 &Ray3::Direction() const
+{
+ return m_kDirection;
+}
+//----------------------------------------------------------------------------
diff --git a/src/editors/FreeMagic/MgcSegment3.h b/src/editors/FreeMagic/MgcSegment3.h
new file mode 100644
index 00000000000..c3638ce3bf2
--- /dev/null
+++ b/src/editors/FreeMagic/MgcSegment3.h
@@ -0,0 +1,43 @@
+// Magic Software, Inc.
+// http://www.magic-software.com
+// Copyright (c) 2000-2002. All Rights Reserved
+//
+// Source code from Magic Software is supplied under the terms of a license
+// agreement and may not be copied or disclosed except in accordance with the
+// terms of that agreement. The various license agreements may be found at
+// the Magic Software web site. This file is subject to the license
+//
+// FREE SOURCE CODE
+// http://www.magic-software.com/License/free.pdf
+
+#ifndef MGCSEGMENT3_H
+#define MGCSEGMENT3_H
+
+#include "MgcVector3.h"
+
+namespace Mgc
+{
+
+ class MAGICFM Segment3
+ {
+ public:
+ // Segment is S(t) = P+t*D for 0 <= t <= 1. D is not necessarily unit
+ // length. The end points are P and P+D.
+ Segment3();
+
+ Vector3 &Origin();
+ const Vector3 &Origin() const;
+
+ Vector3 &Direction();
+ const Vector3 &Direction() const;
+
+ protected:
+ Vector3 m_kOrigin; // P
+ Vector3 m_kDirection; // D
+ };
+
+#include "MgcSegment3.inl"
+
+} // namespace Mgc
+
+#endif
diff --git a/src/editors/FreeMagic/MgcSegment3.inl b/src/editors/FreeMagic/MgcSegment3.inl
new file mode 100644
index 00000000000..c547e0442c9
--- /dev/null
+++ b/src/editors/FreeMagic/MgcSegment3.inl
@@ -0,0 +1,38 @@
+// Magic Software, Inc.
+// http://www.magic-software.com
+// Copyright (c) 2000-2002. All Rights Reserved
+//
+// Source code from Magic Software is supplied under the terms of a license
+// agreement and may not be copied or disclosed except in accordance with the
+// terms of that agreement. The various license agreements may be found at
+// the Magic Software web site. This file is subject to the license
+//
+// FREE SOURCE CODE
+// http://www.magic-software.com/License/free.pdf
+
+//----------------------------------------------------------------------------
+inline Segment3::Segment3()
+{
+ // no initialization for efficiency
+}
+//----------------------------------------------------------------------------
+inline Vector3 &Segment3::Origin()
+{
+ return m_kOrigin;
+}
+//----------------------------------------------------------------------------
+inline const Vector3 &Segment3::Origin() const
+{
+ return m_kOrigin;
+}
+//----------------------------------------------------------------------------
+inline Vector3 &Segment3::Direction()
+{
+ return m_kDirection;
+}
+//----------------------------------------------------------------------------
+inline const Vector3 &Segment3::Direction() const
+{
+ return m_kDirection;
+}
+//----------------------------------------------------------------------------
diff --git a/src/editors/FreeMagic/MgcSmallSet.h b/src/editors/FreeMagic/MgcSmallSet.h
new file mode 100644
index 00000000000..d96640f5426
--- /dev/null
+++ b/src/editors/FreeMagic/MgcSmallSet.h
@@ -0,0 +1,73 @@
+// Magic Software, Inc.
+// http://www.magic-software.com
+// Copyright (c) 2000-2002. All Rights Reserved
+//
+// Source code from Magic Software is supplied under the terms of a license
+// agreement and may not be copied or disclosed except in accordance with the
+// terms of that agreement. The various license agreements may be found at
+// the Magic Software web site. This file is subject to the license
+//
+// FREE SOURCE CODE
+// http://www.magic-software.com/License/free.pdf
+
+#ifndef MGCSMALLSET_H
+#define MGCSMALLSET_H
+
+// This template class is used by Mgc::TriangleMesh to store the edges and
+// triangles that share a vertex and to store the triangles that share an
+// edge. Before this the 'set' class was used from STL, but the memory
+// overhead for the class is enormous. As an example, I used the code to
+// build the adjacency information for a level surface that was a collection
+// of 825K vertices and 1.6M triangles (mesh had 2.5M edges). The memory
+// used by the program that built the adjacency information was 980M. The
+// program based on Mgc::SmallSet used 503M.
+//
+// The idea is that for a typical mesh, the average number of edges sharing
+// a vertex is 6 and the average number of triangles sharing a vertex is 6.
+// The average number of triangles sharing an edge is 2. Since the sets of
+// adjacent objects are small, the Insert method does a linear search to
+// check if the element already exists. Reallocation occurs if the current
+// set is full to make room for the new element. The Remove method does a
+// linear search and, if the requested element is found, removes it and
+// shifts the higher-indexed array elements to fill in the gap. Because of
+// the linear searches, this class should not be used for large sets. Large
+// sets are better handled by a hashed data structure.
+
+#include "MgcRTLib.h"
+
+namespace Mgc
+{
+
+ template
+ class SmallSet
+ {
+ public:
+ SmallSet();
+ SmallSet(int iCapacity, int iGrowBy);
+ SmallSet(const SmallSet &rkSet);
+ ~SmallSet();
+
+ SmallSet &operator=(const SmallSet &rkSet);
+
+ int GetCapacity() const;
+ int GetGrowBy() const;
+ int GetSize() const;
+ const T *GetElements() const;
+ const T &operator[](int i) const;
+
+ bool Insert(const T &rkElement);
+ void InsertNoCheck(const T &rkElement);
+ bool Remove(const T &rkElement);
+ bool Exists(const T &rkElement);
+ void Clear(int iCapacity, int iGrowBy);
+
+ protected:
+ int m_iCapacity, m_iGrowBy, m_iSize;
+ T *m_atElement;
+ };
+
+#include "MgcSmallSet.inl"
+
+} // namespace Mgc
+
+#endif
diff --git a/src/editors/FreeMagic/MgcSmallSet.inl b/src/editors/FreeMagic/MgcSmallSet.inl
new file mode 100644
index 00000000000..bc223a2145a
--- /dev/null
+++ b/src/editors/FreeMagic/MgcSmallSet.inl
@@ -0,0 +1,176 @@
+// Magic Software, Inc.
+// http://www.magic-software.com
+// Copyright (c) 2000-2002. All Rights Reserved
+//
+// Source code from Magic Software is supplied under the terms of a license
+// agreement and may not be copied or disclosed except in accordance with the
+// terms of that agreement. The various license agreements may be found at
+// the Magic Software web site. This file is subject to the license
+//
+// FREE SOURCE CODE
+// http://www.magic-software.com/License/free.pdf
+
+//----------------------------------------------------------------------------
+template
+SmallSet::SmallSet()
+{
+ m_iCapacity = 1;
+ m_iGrowBy = 1;
+ m_iSize = 0;
+ m_atElement = new T[1];
+}
+//----------------------------------------------------------------------------
+template
+SmallSet::SmallSet(int iCapacity, int iGrowBy)
+{
+ assert(iCapacity > 0 && iGrowBy > 0);
+
+ m_iCapacity = iCapacity;
+ m_iGrowBy = iGrowBy;
+ m_iSize = 0;
+ m_atElement = new T[iCapacity];
+}
+//----------------------------------------------------------------------------
+template
+SmallSet::SmallSet(const SmallSet &rkSet)
+{
+ m_iCapacity = rkSet.m_iCapacity;
+ m_iGrowBy = rkSet.m_iGrowBy;
+ m_iSize = rkSet.m_iSize;
+ m_atElement = new T[m_iCapacity];
+ memcpy(m_atElement, rkSet.m_atElement, m_iCapacity * sizeof(T));
+}
+//----------------------------------------------------------------------------
+template
+SmallSet::~SmallSet()
+{
+ delete[] m_atElement;
+}
+//----------------------------------------------------------------------------
+template
+SmallSet &SmallSet::operator=(const SmallSet &rkSet)
+{
+ delete[] m_atElement;
+ m_iCapacity = rkSet.m_iCapacity;
+ m_iGrowBy = rkSet.m_iGrowBy;
+ m_iSize = rkSet.m_iSize;
+ m_atElement = new T[m_iCapacity];
+ memcpy(m_atElement, rkSet.m_atElement, m_iCapacity * sizeof(T));
+ return *this;
+}
+//----------------------------------------------------------------------------
+template
+int SmallSet::GetCapacity() const
+{
+ return m_iCapacity;
+}
+//----------------------------------------------------------------------------
+template
+int SmallSet::GetGrowBy() const
+{
+ return m_iGrowBy;
+}
+//----------------------------------------------------------------------------
+template
+int SmallSet::GetSize() const
+{
+ return m_iSize;
+}
+//----------------------------------------------------------------------------
+template
+const T *SmallSet::GetElements() const
+{
+ return m_atElement;
+}
+//----------------------------------------------------------------------------
+template
+const T &SmallSet::operator[](int i) const
+{
+ assert(0 <= i && i < m_iSize);
+ return m_atElement[i];
+}
+//----------------------------------------------------------------------------
+template
+bool SmallSet::Insert(const T &rkElement)
+{
+ for (int i = 0; i < m_iSize; i++)
+ {
+ if (rkElement == m_atElement[i])
+ return false;
+ }
+
+ if (m_iSize == m_iCapacity)
+ {
+ // array is full, resize it
+ int iNewCapacity = m_iCapacity + m_iGrowBy;
+ T *atNewElement = new T[iNewCapacity];
+ memcpy(atNewElement, m_atElement, m_iCapacity * sizeof(T));
+ delete[] m_atElement;
+ m_atElement = atNewElement;
+ m_iCapacity = iNewCapacity;
+ }
+
+ m_atElement[m_iSize++] = rkElement;
+ return true;
+}
+//----------------------------------------------------------------------------
+template
+void SmallSet::InsertNoCheck(const T &rkElement)
+{
+ if (m_iSize == m_iCapacity)
+ {
+ // array is full, resize it
+ int iNewCapacity = m_iCapacity + m_iGrowBy;
+ T *atNewElement = new T[iNewCapacity];
+ memcpy(atNewElement, m_atElement, m_iCapacity * sizeof(T));
+ delete[] m_atElement;
+ m_atElement = atNewElement;
+ m_iCapacity = iNewCapacity;
+ }
+
+ m_atElement[m_iSize++] = rkElement;
+}
+//----------------------------------------------------------------------------
+template
+bool SmallSet::Remove(const T &rkElement)
+{
+ for (int i = 0; i < m_iSize; i++)
+ {
+ if (rkElement == m_atElement[i])
+ {
+ // element exists, shift array to fill in empty slot
+ for (int j = i + 1; j < m_iSize; j++, i++)
+ m_atElement[i] = m_atElement[j];
+
+ m_iSize--;
+ return true;
+ }
+ }
+
+ return false;
+}
+//----------------------------------------------------------------------------
+template
+bool SmallSet::Exists(const T &rkElement)
+{
+ for (int i = 0; i < m_iSize; i++)
+ {
+ if (rkElement == m_atElement[i])
+ return true;
+ }
+
+ return false;
+}
+//----------------------------------------------------------------------------
+template
+void SmallSet::Clear(int iCapacity, int iGrowBy)
+{
+ assert(iCapacity > 0 && iGrowBy > 0);
+
+ delete[] m_atElement;
+ m_iCapacity = iCapacity;
+ m_iGrowBy = iGrowBy;
+ m_iSize = 0;
+ m_atElement = new T[iCapacity];
+}
+//----------------------------------------------------------------------------
diff --git a/src/editors/FreeMagic/MgcSphere.h b/src/editors/FreeMagic/MgcSphere.h
new file mode 100644
index 00000000000..03f376ca796
--- /dev/null
+++ b/src/editors/FreeMagic/MgcSphere.h
@@ -0,0 +1,41 @@
+// Magic Software, Inc.
+// http://www.magic-software.com
+// Copyright (c) 2000-2002. All Rights Reserved
+//
+// Source code from Magic Software is supplied under the terms of a license
+// agreement and may not be copied or disclosed except in accordance with the
+// terms of that agreement. The various license agreements may be found at
+// the Magic Software web site. This file is subject to the license
+//
+// FREE SOURCE CODE
+// http://www.magic-software.com/License/free.pdf
+
+#ifndef MGCSPHERE_H
+#define MGCSPHERE_H
+
+#include "MgcVector3.h"
+
+namespace Mgc
+{
+
+ class MAGICFM Sphere
+ {
+ public:
+ Sphere();
+
+ Vector3 &Center();
+ const Vector3 &Center() const;
+
+ Real &Radius();
+ const Real &Radius() const;
+
+ protected:
+ Vector3 m_kCenter;
+ Real m_fRadius;
+ };
+
+#include "MgcSphere.inl"
+
+} // namespace Mgc
+
+#endif
diff --git a/src/editors/FreeMagic/MgcSphere.inl b/src/editors/FreeMagic/MgcSphere.inl
new file mode 100644
index 00000000000..beeb466819d
--- /dev/null
+++ b/src/editors/FreeMagic/MgcSphere.inl
@@ -0,0 +1,38 @@
+// Magic Software, Inc.
+// http://www.magic-software.com
+// Copyright (c) 2000-2002. All Rights Reserved
+//
+// Source code from Magic Software is supplied under the terms of a license
+// agreement and may not be copied or disclosed except in accordance with the
+// terms of that agreement. The various license agreements may be found at
+// the Magic Software web site. This file is subject to the license
+//
+// FREE SOURCE CODE
+// http://www.magic-software.com/License/free.pdf
+
+//----------------------------------------------------------------------------
+inline Sphere::Sphere()
+{
+ // no initialization for efficiency
+}
+//----------------------------------------------------------------------------
+inline Vector3 &Sphere::Center()
+{
+ return m_kCenter;
+}
+//----------------------------------------------------------------------------
+inline const Vector3 &Sphere::Center() const
+{
+ return m_kCenter;
+}
+//----------------------------------------------------------------------------
+inline Real &Sphere::Radius()
+{
+ return m_fRadius;
+}
+//----------------------------------------------------------------------------
+inline const Real &Sphere::Radius() const
+{
+ return m_fRadius;
+}
+//----------------------------------------------------------------------------
diff --git a/src/editors/FreeMagic/MgcTriangleMesh.cpp b/src/editors/FreeMagic/MgcTriangleMesh.cpp
new file mode 100644
index 00000000000..5c56e79b705
--- /dev/null
+++ b/src/editors/FreeMagic/MgcTriangleMesh.cpp
@@ -0,0 +1,776 @@
+// Magic Software, Inc.
+// http://www.magic-software.com
+// Copyright (c) 2000-2002. All Rights Reserved
+//
+// Source code from Magic Software is supplied under the terms of a license
+// agreement and may not be copied or disclosed except in accordance with the
+// terms of that agreement. The various license agreements may be found at
+// the Magic Software web site. This file is subject to the license
+//
+// FREE SOURCE CODE
+// http://www.magic-software.com/License/free.pdf
+
+#include "MgcTriangleMesh.h"
+using namespace Mgc;
+
+#include
+#include
+#include
+using namespace std;
+
+#ifdef SGI_MIPSPRO
+// Instantiations to support dynamic link libraries. This appears to be
+// necessary on the SGI running IRIX and using the MIPSPRO CC compiler.
+template class Mgc::SmallSet;
+template class Mgc::SmallSet;
+#endif
+
+//----------------------------------------------------------------------------
+TriangleMesh::TriangleMesh()
+{
+}
+//----------------------------------------------------------------------------
+TriangleMesh::~TriangleMesh()
+{
+}
+//----------------------------------------------------------------------------
+void TriangleMesh::InsertTriangle(int iV0, int iV1, int iV2)
+{
+ Triangle kT(iV0, iV1, iV2);
+ Edge kE0(iV0, iV1), kE1(iV1, iV2), kE2(iV2, iV0);
+
+ // insert triangle
+ pair kRT = m_kTMap.insert(make_pair(kT, TriangleAttribute()));
+
+ // insert vertices
+ pair kRV0 = m_kVMap.insert(make_pair(iV0, VertexAttribute()));
+ kRV0.first->second.m_kESet.Insert(kE0);
+ kRV0.first->second.m_kESet.Insert(kE2);
+ kRV0.first->second.m_kTSet.Insert(kT);
+
+ pair kRV1 = m_kVMap.insert(make_pair(iV1, VertexAttribute()));
+ kRV1.first->second.m_kESet.Insert(kE0);
+ kRV1.first->second.m_kESet.Insert(kE1);
+ kRV1.first->second.m_kTSet.Insert(kT);
+
+ pair kRV2 = m_kVMap.insert(make_pair(iV2, VertexAttribute()));
+ kRV2.first->second.m_kESet.Insert(kE1);
+ kRV2.first->second.m_kESet.Insert(kE2);
+ kRV2.first->second.m_kTSet.Insert(kT);
+
+ // insert edges
+ pair kRE0 = m_kEMap.insert(make_pair(kE0, EdgeAttribute()));
+ kRE0.first->second.m_kTSet.Insert(kT);
+
+ pair kRE1 = m_kEMap.insert(make_pair(kE1, EdgeAttribute()));
+ kRE1.first->second.m_kTSet.Insert(kT);
+
+ pair kRE2 = m_kEMap.insert(make_pair(kE2, EdgeAttribute()));
+ kRE2.first->second.m_kTSet.Insert(kT);
+
+ // Notify derived classes that mesh components have been inserted. The
+ // notification occurs here to make sure the derived classes have access
+ // to the current state of the mesh after the triangle insertion.
+ OnVertexInsert(iV0, kRV0.second, kRV0.first->second.m_pvData);
+ OnVertexInsert(iV1, kRV1.second, kRV1.first->second.m_pvData);
+ OnVertexInsert(iV2, kRV2.second, kRV2.first->second.m_pvData);
+ OnEdgeInsert(kE0, kRE0.second, kRE0.first->second.m_pvData);
+ OnEdgeInsert(kE1, kRE1.second, kRE1.first->second.m_pvData);
+ OnEdgeInsert(kE2, kRE2.second, kRE2.first->second.m_pvData);
+ OnTriangleInsert(kT, kRT.second, kRT.first->second.m_pvData);
+}
+//----------------------------------------------------------------------------
+void TriangleMesh::InsertTriangle(const Triangle &rkT)
+{
+ InsertTriangle(rkT.m_aiV[0], rkT.m_aiV[1], rkT.m_aiV[2]);
+}
+//----------------------------------------------------------------------------
+void TriangleMesh::RemoveTriangle(int iV0, int iV1, int iV2)
+{
+ // remove triangle
+ Triangle kT(iV0, iV1, iV2);
+ MTIter pkT = m_kTMap.find(kT);
+ if (pkT == m_kTMap.end())
+ {
+ // triangle does not exist, nothing to do
+ return;
+ }
+
+ // update edges
+ Edge kE0(iV0, iV1), kE1(iV1, iV2), kE2(iV2, iV0);
+
+ MEIter pkE0 = m_kEMap.find(kE0);
+ assert(pkE0 != m_kEMap.end());
+ pkE0->second.m_kTSet.Remove(kT);
+
+ MEIter pkE1 = m_kEMap.find(kE1);
+ assert(pkE1 != m_kEMap.end());
+ pkE1->second.m_kTSet.Remove(kT);
+
+ MEIter pkE2 = m_kEMap.find(kE2);
+ assert(pkE2 != m_kEMap.end());
+ pkE2->second.m_kTSet.Remove(kT);
+
+ // update vertices
+ MVIter pkV0 = m_kVMap.find(iV0);
+ assert(pkV0 != m_kVMap.end());
+ pkV0->second.m_kTSet.Remove(kT);
+
+ MVIter pkV1 = m_kVMap.find(iV1);
+ assert(pkV1 != m_kVMap.end());
+ pkV1->second.m_kTSet.Remove(kT);
+
+ MVIter pkV2 = m_kVMap.find(iV2);
+ assert(pkV2 != m_kVMap.end());
+ pkV2->second.m_kTSet.Remove(kT);
+
+ if (pkE0->second.m_kTSet.GetSize() == 0)
+ {
+ pkV0->second.m_kESet.Remove(kE0);
+ pkV1->second.m_kESet.Remove(kE0);
+ }
+
+ if (pkE1->second.m_kTSet.GetSize() == 0)
+ {
+ pkV1->second.m_kESet.Remove(kE1);
+ pkV2->second.m_kESet.Remove(kE1);
+ }
+
+ if (pkE2->second.m_kTSet.GetSize() == 0)
+ {
+ pkV0->second.m_kESet.Remove(kE2);
+ pkV2->second.m_kESet.Remove(kE2);
+ }
+
+ // Notify derived classes that mesh components are about to be destroyed.
+ // The notification occurs here to make sure the derived classes have
+ // access to the current state of the mesh before the triangle removal.
+
+ bool bDestroy = pkV0->second.m_kESet.GetSize() == 0 &&
+ pkV0->second.m_kTSet.GetSize() == 0;
+ OnVertexRemove(iV0, bDestroy, pkV0->second.m_pvData);
+ if (bDestroy)
+ m_kVMap.erase(iV0);
+
+ bDestroy = pkV1->second.m_kESet.GetSize() == 0 &&
+ pkV1->second.m_kTSet.GetSize() == 0;
+ OnVertexRemove(iV1, bDestroy, pkV1->second.m_pvData);
+ if (bDestroy)
+ m_kVMap.erase(iV1);
+
+ bDestroy = pkV2->second.m_kESet.GetSize() == 0 &&
+ pkV2->second.m_kTSet.GetSize() == 0;
+ OnVertexRemove(iV2, bDestroy, pkV2->second.m_pvData);
+ if (bDestroy)
+ m_kVMap.erase(iV2);
+
+ bDestroy = pkE0->second.m_kTSet.GetSize() == 0;
+ OnEdgeRemove(kE0, bDestroy, pkE0->second.m_pvData);
+ if (bDestroy)
+ m_kEMap.erase(kE0);
+
+ bDestroy = pkE1->second.m_kTSet.GetSize() == 0;
+ OnEdgeRemove(kE1, bDestroy, pkE1->second.m_pvData);
+ if (bDestroy)
+ m_kEMap.erase(kE1);
+
+ bDestroy = pkE2->second.m_kTSet.GetSize() == 0;
+ OnEdgeRemove(kE2, bDestroy, pkE2->second.m_pvData);
+ if (bDestroy)
+ m_kEMap.erase(kE2);
+
+ OnTriangleRemove(kT, true, pkT->second.m_pvData);
+ m_kTMap.erase(kT);
+}
+//----------------------------------------------------------------------------
+void TriangleMesh::RemoveTriangle(const Triangle &rkT)
+{
+ RemoveTriangle(rkT.m_aiV[0], rkT.m_aiV[1], rkT.m_aiV[2]);
+}
+//----------------------------------------------------------------------------
+void TriangleMesh::RemoveAllTriangles()
+{
+ MTIter pkT = m_kTMap.begin();
+ while (pkT != m_kTMap.end())
+ {
+ int iV0 = pkT->first.m_aiV[0];
+ int iV1 = pkT->first.m_aiV[1];
+ int iV2 = pkT->first.m_aiV[2];
+ pkT++;
+
+ RemoveTriangle(iV0, iV1, iV2);
+ }
+}
+//----------------------------------------------------------------------------
+void TriangleMesh::Print(const char *acFilename) const
+{
+ ofstream kOStr(acFilename);
+ int i;
+
+ // print vertices
+ kOStr << "vertex quantity = " << m_kVMap.size() << endl;
+ for (MVCIter pkVM = m_kVMap.begin(); pkVM != m_kVMap.end(); pkVM++)
+ {
+ kOStr << "v<" << pkVM->first << "> : e ";
+
+ const SmallSet &rkESet = pkVM->second.m_kESet;
+ for (i = 0; i < rkESet.GetSize(); i++)
+ {
+ kOStr << '<' << rkESet[i].m_aiV[0]
+ << ',' << rkESet[i].m_aiV[1]
+ << "> ";
+ }
+
+ kOStr << ": t ";
+ const SmallSet &rkTSet = pkVM->second.m_kTSet;
+ for (i = 0; i < rkTSet.GetSize(); i++)
+ {
+ kOStr << '<' << rkTSet[i].m_aiV[0]
+ << ',' << rkTSet[i].m_aiV[1]
+ << ',' << rkTSet[i].m_aiV[2]
+ << "> ";
+ }
+ kOStr << endl;
+ }
+ kOStr << endl;
+
+ // print edges
+ kOStr << "edge quantity = " << m_kEMap.size() << endl;
+ for (MECIter pkEM = m_kEMap.begin(); pkEM != m_kEMap.end(); pkEM++)
+ {
+ kOStr << "e<" << pkEM->first.m_aiV[0] << ',' << pkEM->first.m_aiV[1];
+ kOStr << "> : t ";
+ const SmallSet &rkTSet = pkEM->second.m_kTSet;
+ for (i = 0; i < rkTSet.GetSize(); i++)
+ {
+ kOStr << '<' << rkTSet[i].m_aiV[0]
+ << ',' << rkTSet[i].m_aiV[1]
+ << ',' << rkTSet[i].m_aiV[2]
+ << "> ";
+ }
+ kOStr << endl;
+ }
+ kOStr << endl;
+
+ // print triangles
+ kOStr << "triangle quantity = " << m_kTMap.size() << endl;
+ for (MTCIter pkTM = m_kTMap.begin(); pkTM != m_kTMap.end(); pkTM++)
+ {
+ kOStr << "t<" << pkTM->first.m_aiV[0] << ',' << pkTM->first.m_aiV[1];
+ kOStr << ',' << pkTM->first.m_aiV[2] << ">" << endl;
+ }
+ kOStr << endl;
+}
+//----------------------------------------------------------------------------
+void TriangleMesh::GetVertices(set &rkVSet) const
+{
+ rkVSet.clear();
+ for (MVCIter pkV = m_kVMap.begin(); pkV != m_kVMap.end(); pkV++)
+ rkVSet.insert(pkV->first);
+}
+//----------------------------------------------------------------------------
+void *TriangleMesh::GetData(int iV)
+{
+ MVIter pkV = m_kVMap.find(iV);
+ return (pkV != m_kVMap.end() ? pkV->second.m_pvData : NULL);
+}
+//----------------------------------------------------------------------------
+const SmallSet *TriangleMesh::GetEdges(int iV) const
+{
+ MVCIter pkV = m_kVMap.find(iV);
+ return (pkV != m_kVMap.end() ? &pkV->second.m_kESet : NULL);
+}
+//----------------------------------------------------------------------------
+const SmallSet *TriangleMesh::GetTriangles(int iV)
+ const
+{
+ MVCIter pkV = m_kVMap.find(iV);
+ return (pkV != m_kVMap.end() ? &pkV->second.m_kTSet : NULL);
+}
+//----------------------------------------------------------------------------
+void TriangleMesh::GetEdges(set &rkESet) const
+{
+ rkESet.clear();
+ for (MECIter pkE = m_kEMap.begin(); pkE != m_kEMap.end(); pkE++)
+ rkESet.insert(pkE->first);
+}
+//----------------------------------------------------------------------------
+void *TriangleMesh::GetData(int iV0, int iV1)
+{
+ MEIter pkE = m_kEMap.find(Edge(iV0, iV1));
+ return (pkE != m_kEMap.end() ? pkE->second.m_pvData : NULL);
+}
+//----------------------------------------------------------------------------
+void *TriangleMesh::GetData(const Edge &rkE)
+{
+ return GetData(rkE.m_aiV[0], rkE.m_aiV[1]);
+}
+//----------------------------------------------------------------------------
+const SmallSet *TriangleMesh::GetTriangles(int iV0,
+ int iV1) const
+{
+ MECIter pkE = m_kEMap.find(Edge(iV0, iV1));
+ return (pkE != m_kEMap.end() ? &pkE->second.m_kTSet : NULL);
+}
+//----------------------------------------------------------------------------
+void TriangleMesh::GetTriangles(set &rkTSet) const
+{
+ rkTSet.clear();
+ for (MTCIter pkT = m_kTMap.begin(); pkT != m_kTMap.end(); pkT++)
+ rkTSet.insert(pkT->first);
+}
+//----------------------------------------------------------------------------
+void *TriangleMesh::GetData(int iV0, int iV1, int iV2)
+{
+ MTIter pkT = m_kTMap.find(Triangle(iV0, iV1, iV2));
+ return (pkT != m_kTMap.end() ? pkT->second.m_pvData : NULL);
+}
+//----------------------------------------------------------------------------
+void *TriangleMesh::GetData(const Triangle &rkT)
+{
+ return GetData(rkT.m_aiV[0], rkT.m_aiV[1], rkT.m_aiV[2]);
+}
+//----------------------------------------------------------------------------
+bool TriangleMesh::IsManifold() const
+{
+ for (MECIter pkE = m_kEMap.begin(); pkE != m_kEMap.end(); pkE++)
+ {
+ if (pkE->second.m_kTSet.GetSize() > 2)
+ return false;
+ }
+ return true;
+}
+//----------------------------------------------------------------------------
+bool TriangleMesh::IsClosed() const
+{
+ for (MECIter pkE = m_kEMap.begin(); pkE != m_kEMap.end(); pkE++)
+ {
+ if (pkE->second.m_kTSet.GetSize() != 2)
+ return false;
+ }
+ return true;
+}
+//----------------------------------------------------------------------------
+bool TriangleMesh::IsConnected() const
+{
+ // Do a depth-first search of the mesh. It is connected if and only if
+ // all of the triangles are visited on a single search.
+
+ int iTSize = m_kTMap.size();
+ if (iTSize == 0)
+ return true;
+
+ // for marking visited triangles during the traversal
+ map kVisitedMap;
+ MTCIter pkT;
+ for (pkT = m_kTMap.begin(); pkT != m_kTMap.end(); pkT++)
+ kVisitedMap.insert(make_pair(pkT->first, false));
+
+ // start the traversal at any triangle in the mesh
+ stack kStack;
+ kStack.push(m_kTMap.begin()->first);
+ map::iterator pkVI =
+ kVisitedMap.find(m_kTMap.begin()->first);
+ assert(pkVI != kVisitedMap.end());
+ pkVI->second = true;
+ iTSize--;
+
+ while (!kStack.empty())
+ {
+ // start at the current triangle
+ Triangle kT = kStack.top();
+ kStack.pop();
+
+ for (int i = 0; i < 3; i++)
+ {
+ // get an edge of the current triangle
+ MECIter pkE = m_kEMap.find(Edge(kT.m_aiV[i], kT.m_aiV[(i + 1) % 3]));
+
+ // visit each adjacent triangle
+ const SmallSet &rkTSet = pkE->second.m_kTSet;
+ for (int j = 0; j < rkTSet.GetSize(); j++)
+ {
+ const Triangle &rkTAdj = rkTSet[j];
+ pkVI = kVisitedMap.find(rkTAdj);
+ assert(pkVI != kVisitedMap.end());
+ if (pkVI->second == false)
+ {
+ // this adjacent triangle not yet visited
+ kStack.push(rkTAdj);
+ pkVI->second = true;
+ iTSize--;
+ }
+ }
+ }
+ }
+
+ return iTSize == 0;
+}
+//----------------------------------------------------------------------------
+void TriangleMesh::GetComponents(vector &rkComponents)
+{
+ // Do a depth-first search of the mesh to find connected components.
+ int iTSize = m_kTMap.size();
+ if (iTSize == 0)
+ return;
+
+ // for marking visited triangles during the traversal
+ map kVisitedMap;
+ MTCIter pkT;
+ for (pkT = m_kTMap.begin(); pkT != m_kTMap.end(); pkT++)
+ kVisitedMap.insert(make_pair(pkT->first, false));
+
+ while (iTSize > 0)
+ {
+ // find an unvisited triangle in the mesh
+ stack kStack;
+ map::iterator pkVI = kVisitedMap.begin();
+ while (pkVI != kVisitedMap.end())
+ {
+ if (pkVI->second == false)
+ {
+ kStack.push(pkVI->first);
+ pkVI->second = true;
+ iTSize--;
+ break;
+ }
+ pkVI++;
+ }
+
+ // traverse the connected component of the starting triangle
+ TriangleMesh *pkComponent = Create();
+ while (!kStack.empty())
+ {
+ // start at the current triangle
+ Triangle kT = kStack.top();
+ kStack.pop();
+ pkComponent->InsertTriangle(kT);
+
+ for (int i = 0; i < 3; i++)
+ {
+ // get an edge of the current triangle
+ Edge kE(kT.m_aiV[i], kT.m_aiV[(i + 1) % 3]);
+ MECIter pkE = m_kEMap.find(kE);
+
+ // visit each adjacent triangle
+ const SmallSet &rkTSet = pkE->second.m_kTSet;
+ for (int j = 0; j < rkTSet.GetSize(); j++)
+ {
+ const Triangle &rkTAdj = rkTSet[i];
+ pkVI = kVisitedMap.find(rkTAdj);
+ assert(pkVI != kVisitedMap.end());
+ if (pkVI->second == false)
+ {
+ // this adjacent triangle not yet visited
+ kStack.push(rkTAdj);
+ pkVI->second = true;
+ iTSize--;
+ }
+ }
+ }
+ }
+ rkComponents.push_back(pkComponent);
+ }
+}
+//----------------------------------------------------------------------------
+void TriangleMesh::GetComponents(vector &rkIndex, int *&raiConnect)
+{
+ rkIndex.clear();
+
+ // Do a depth-first search of the mesh to find connected components.
+ int iTSize = m_kTMap.size();
+ if (iTSize == 0)
+ {
+ raiConnect = NULL;
+ return;
+ }
+
+ int iIQuantity = 3 * iTSize;
+ int iIndex = 0;
+ raiConnect = new int[iIQuantity];
+
+ // for marking visited triangles during the traversal
+ map kVisitedMap;
+ MTCIter pkT;
+ for (pkT = m_kTMap.begin(); pkT != m_kTMap.end(); pkT++)
+ kVisitedMap.insert(make_pair(pkT->first, false));
+
+ while (iTSize > 0)
+ {
+ // find an unvisited triangle in the mesh
+ stack kStack;
+ map::iterator pkVI = kVisitedMap.begin();
+ while (pkVI != kVisitedMap.end())
+ {
+ if (pkVI->second == false)
+ {
+ kStack.push(pkVI->first);
+ pkVI->second = true;
+ iTSize--;
+ break;
+ }
+ pkVI++;
+ }
+
+ // traverse the connected component of the starting triangle
+ TriangleMesh *pkComponent = Create();
+ while (!kStack.empty())
+ {
+ // start at the current triangle
+ Triangle kT = kStack.top();
+ kStack.pop();
+ pkComponent->InsertTriangle(kT);
+
+ for (int i = 0; i < 3; i++)
+ {
+ // get an edge of the current triangle
+ Edge kE(kT.m_aiV[i], kT.m_aiV[(i + 1) % 3]);
+ MECIter pkE = m_kEMap.find(kE);
+
+ // visit each adjacent triangle
+ const SmallSet &rkTSet = pkE->second.m_kTSet;
+ for (int j = 0; j < rkTSet.GetSize(); j++)
+ {
+ const Triangle &rkTAdj = rkTSet[i];
+ pkVI = kVisitedMap.find(rkTAdj);
+ assert(pkVI != kVisitedMap.end());
+ if (pkVI->second == false)
+ {
+ // this adjacent triangle not yet visited
+ kStack.push(rkTAdj);
+ pkVI->second = true;
+ iTSize--;
+ }
+ }
+ }
+ }
+
+ // store the connectivity information for this component
+ set kTSet;
+ pkComponent->GetTriangles(kTSet);
+ delete pkComponent;
+
+ rkIndex.push_back(iIndex);
+ set::iterator pkTIter;
+ for (pkTIter = kTSet.begin(); pkTIter != kTSet.end(); pkTIter++)
+ {
+ assert(iIndex + 3 <= iIQuantity);
+ const Triangle &rkT = *pkTIter;
+ raiConnect[iIndex++] = rkT.m_aiV[0];
+ raiConnect[iIndex++] = rkT.m_aiV[1];
+ raiConnect[iIndex++] = rkT.m_aiV[2];
+ }
+ }
+
+ rkIndex.push_back(iIQuantity);
+}
+//----------------------------------------------------------------------------
+void TriangleMesh::RemoveComponent(int &riIQuantity, int *aiConnect)
+{
+ // Do a depth-first search of the mesh to find connected components. The
+ // input array is assumed to be large enough to hold the component (see
+ // the comments in MgcTriangleMesh.h for RemoveComponent).
+ riIQuantity = 0;
+
+ int iTSize = m_kTMap.size();
+ if (iTSize == 0)
+ return;
+
+ // Find the connected component containing the first triangle in the mesh.
+ // A set is used instead of a stack to avoid having a large-memory
+ // 'visited' map.
+ set kVisited;
+ kVisited.insert(m_kTMap.begin()->first);
+
+ // traverse the connected component
+ while (!kVisited.empty())
+ {
+ // start at the current triangle
+ Triangle kT = *kVisited.begin();
+
+ // add adjacent triangles to the set for recursive processing
+ for (int i = 0; i < 3; i++)
+ {
+ // get an edge of the current triangle
+ Edge kE(kT.m_aiV[i], kT.m_aiV[(i + 1) % 3]);
+ MECIter pkE = m_kEMap.find(kE);
+ assert(pkE != m_kEMap.end());
+
+ // visit each adjacent triangle
+ const SmallSet &rkTSet = pkE->second.m_kTSet;
+ for (int j = 0; j < rkTSet.GetSize(); j++)
+ {
+ Triangle kTAdj = rkTSet[j];
+ if (kTAdj != kT)
+ kVisited.insert(kTAdj);
+ }
+ }
+
+ // add triangle to connectivity array
+ aiConnect[riIQuantity++] = kT.m_aiV[0];
+ aiConnect[riIQuantity++] = kT.m_aiV[1];
+ aiConnect[riIQuantity++] = kT.m_aiV[2];
+
+ // remove the current triangle (visited, no longer needed)
+ kVisited.erase(kT);
+ RemoveTriangle(kT);
+ }
+}
+//----------------------------------------------------------------------------
+bool TriangleMesh::GetConsistentComponents(
+ vector &rkComponents)
+{
+ if (!IsManifold())
+ return false;
+
+ // Do a depth-first search of the mesh to find connected components.
+ int iTSize = m_kTMap.size();
+ if (iTSize == 0)
+ return true;
+
+ // for marking visited triangles during the traversal
+ map kVisitedMap;
+ MTCIter pkT;
+ for (pkT = m_kTMap.begin(); pkT != m_kTMap.end(); pkT++)
+ kVisitedMap.insert(make_pair(pkT->first, false));
+
+ while (iTSize > 0)
+ {
+ // Find an unvisited triangle in the mesh. Any triangle pushed onto
+ // the stack is considered to have a consistent ordering.
+ stack kStack;
+ map::iterator pkVI = kVisitedMap.begin();
+ while (pkVI != kVisitedMap.end())
+ {
+ if (pkVI->second == false)
+ {
+ kStack.push(pkVI->first);
+ pkVI->second = true;
+ iTSize--;
+ break;
+ }
+ pkVI++;
+ }
+
+ // traverse the connected component of the starting triangle
+ TriangleMesh *pkComponent = Create();
+ while (!kStack.empty())
+ {
+ // start at the current triangle
+ Triangle kT = kStack.top();
+ kStack.pop();
+ pkComponent->InsertTriangle(kT);
+
+ for (int i = 0; i < 3; i++)
+ {
+ // get an edge of the current triangle
+ int iV0 = kT.m_aiV[i], iV1 = kT.m_aiV[(i + 1) % 3], iV2;
+ Edge kE(iV0, iV1);
+ MECIter pkE = m_kEMap.find(kE);
+
+ int iSize = pkE->second.m_kTSet.GetSize();
+ assert(iSize == 1 || iSize == 2); // mesh is manifold
+ const Triangle *pkTAdj = &pkE->second.m_kTSet[0];
+ if (iSize == 2)
+ {
+ // get the adjacent triangle to the current one
+ if (*pkTAdj == kT)
+ pkTAdj = &pkE->second.m_kTSet[1];
+
+ pkVI = kVisitedMap.find(*pkTAdj);
+ assert(pkVI != kVisitedMap.end());
+ if (pkVI->second == false)
+ {
+ // adjacent triangle not yet visited
+ if ((pkTAdj->m_aiV[0] == iV0 && pkTAdj->m_aiV[1] == iV1) || (pkTAdj->m_aiV[1] == iV0 && pkTAdj->m_aiV[2] == iV1) || (pkTAdj->m_aiV[2] == iV0 && pkTAdj->m_aiV[0] == iV1))
+ {
+ // adjacent triangle must be reordered
+ iV0 = pkTAdj->m_aiV[0];
+ iV1 = pkTAdj->m_aiV[1];
+ iV2 = pkTAdj->m_aiV[2];
+ kVisitedMap.erase(*pkTAdj);
+ RemoveTriangle(iV0, iV1, iV2);
+ InsertTriangle(iV1, iV0, iV2);
+ kVisitedMap.insert(make_pair(Triangle(iV1, iV0,
+ iV2),
+ false));
+
+ // refresh the iterators since maps changed
+ pkE = m_kEMap.find(kE);
+ pkTAdj = &pkE->second.m_kTSet[0];
+ if (*pkTAdj == kT)
+ pkTAdj = &pkE->second.m_kTSet[1];
+ pkVI = kVisitedMap.find(*pkTAdj);
+ assert(pkVI != kVisitedMap.end());
+ }
+
+ kStack.push(*pkTAdj);
+ pkVI->second = true;
+ iTSize--;
+ }
+ }
+ }
+ }
+ rkComponents.push_back(pkComponent);
+ }
+
+ return true;
+}
+//----------------------------------------------------------------------------
+TriangleMesh *TriangleMesh::GetReversedOrderMesh() const
+{
+ TriangleMesh *pkReversed = Create();
+
+ for (MTCIter pkT = m_kTMap.begin(); pkT != m_kTMap.end(); pkT++)
+ {
+ pkReversed->InsertTriangle(pkT->first.m_aiV[0], pkT->first.m_aiV[2],
+ pkT->first.m_aiV[1]);
+ }
+
+ return pkReversed;
+}
+//----------------------------------------------------------------------------
+void TriangleMesh::GetStatistics(int &riVQuantity, int &riEQuantity,
+ int &riTQuantity, Real &rfAverageEdgesPerVertex,
+ Real &rfAverageTrianglesPerVertex, Real &rfAverageTrianglesPerEdge,
+ int &riMaximumEdgesPerVertex, int &riMaximumTrianglesPerVertex,
+ int &riMaximumTrianglesPerEdge)
+{
+ riVQuantity = m_kVMap.size();
+ riEQuantity = m_kEMap.size();
+ riTQuantity = m_kTMap.size();
+
+ int iESumForV = 0;
+ int iTSumForV = 0;
+ riMaximumEdgesPerVertex = 0;
+ riMaximumTrianglesPerVertex = 0;
+
+ int iESize, iTSize;
+
+ for (MVCIter pkV = m_kVMap.begin(); pkV != m_kVMap.end(); pkV++)
+ {
+ iESize = pkV->second.m_kESet.GetSize();
+ iTSize = pkV->second.m_kTSet.GetSize();
+ iESumForV += iESize;
+ iTSumForV += iTSize;
+ if (iESize > riMaximumEdgesPerVertex)
+ riMaximumEdgesPerVertex = iESize;
+ if (iTSize > riMaximumTrianglesPerVertex)
+ riMaximumTrianglesPerVertex = iTSize;
+ }
+
+ int iTSumForE = 0;
+ riMaximumTrianglesPerEdge = 0;
+ for (MECIter pkE = m_kEMap.begin(); pkE != m_kEMap.end(); pkE++)
+ {
+ iTSize = pkE->second.m_kTSet.GetSize();
+ iTSumForE += iTSize;
+ if (iTSize > riMaximumTrianglesPerEdge)
+ riMaximumTrianglesPerEdge = iTSize;
+ }
+
+ rfAverageEdgesPerVertex = ((Real)iESumForV) / riVQuantity;
+ rfAverageTrianglesPerVertex = ((Real)iTSumForV) / riVQuantity;
+ rfAverageTrianglesPerEdge = ((Real)iTSumForE) / riEQuantity;
+}
+//----------------------------------------------------------------------------
diff --git a/src/editors/FreeMagic/MgcTriangleMesh.h b/src/editors/FreeMagic/MgcTriangleMesh.h
new file mode 100644
index 00000000000..49c9faaaab6
--- /dev/null
+++ b/src/editors/FreeMagic/MgcTriangleMesh.h
@@ -0,0 +1,254 @@
+// Magic Software, Inc.
+// http://www.magic-software.com
+// Copyright (c) 2000-2002. All Rights Reserved
+//
+// Source code from Magic Software is supplied under the terms of a license
+// agreement and may not be copied or disclosed except in accordance with the
+// terms of that agreement. The various license agreements may be found at
+// the Magic Software web site. This file is subject to the license
+//
+// FREE SOURCE CODE
+// http://www.magic-software.com/License/free.pdf
+
+#ifndef MGCTRIANGLEMESH_H
+#define MGCTRIANGLEMESH_H
+
+#include "MagicFMLibType.h"
+#include "MgcMath.h"
+#include "MgcSmallSet.h"
+#include