Skip to content
GitLab
Projects
Groups
Snippets
/
Help
Help
Support
Community forum
Keyboard shortcuts
?
Submit feedback
Contribute to GitLab
Sign in / Register
Toggle navigation
Menu
Open sidebar
Guilhem Saurel
hpp-fcl
Commits
2dbd2392
Commit
2dbd2392
authored
Nov 28, 2018
by
Florent Lamiraux
Committed by
Florent Lamiraux florent@laas.fr
Dec 06, 2018
Browse files
Add a test on geometric tools and comparison with fcl GJK solver.
parent
4c14cc3e
Changes
17
Expand all
Hide whitespace changes
Inline
Side-by-side
src/geometric-tools/GTEngineDEF.h
0 → 100644
View file @
2dbd2392
// David Eberly, Geometric Tools, Redmond WA 98052
// Copyright (c) 1998-2018
// Distributed under the Boost Software License, Version 1.0.
// http://www.boost.org/LICENSE_1_0.txt
// http://www.geometrictools.com/License/Boost/LICENSE_1_0.txt
// File Version: 3.0.0 (2016/06/19)
#pragma once
//----------------------------------------------------------------------------
// The platform specification.
//
// __MSWINDOWS__ : Microsoft Windows (WIN32 or WIN64)
// __APPLE__ : Macintosh OS X
// __LINUX__ : Linux or Cygwin
//----------------------------------------------------------------------------
#if !defined(__LINUX__) && (defined(WIN32) || defined(_WIN64))
#define __MSWINDOWS__
#if !defined(_MSC_VER)
#error Microsoft Visual Studio 2013 or later is required.
#endif
// MSVC 6 is version 12.0
// MSVC 7.0 is version 13.0 (MSVS 2002)
// MSVC 7.1 is version 13.1 (MSVS 2003)
// MSVC 8.0 is version 14.0 (MSVS 2005)
// MSVC 9.0 is version 15.0 (MSVS 2008)
// MSVC 10.0 is version 16.0 (MSVS 2010)
// MSVC 11.0 is version 17.0 (MSVS 2012)
// MSVC 12.0 is version 18.0 (MSVS 2013)
// MSVC 14.0 is version 19.0 (MSVS 2015)
// Currently, projects are provided only for MSVC 12.0 and 14.0.
#if _MSC_VER < 1800
#error Microsoft Visual Studio 2013 or later is required.
#endif
// Debug build values (choose_your_value is 0, 1, or 2)
// 0: Disables checked iterators and disables iterator debugging.
// 1: Enables checked iterators and disables iterator debugging.
// 2: (default) Enables iterator debugging; checked iterators are not relevant.
//
// Release build values (choose_your_value is 0 or 1)
// 0: (default) Disables checked iterators.
// 1: Enables checked iterators; iterator debugging is not relevant.
//
// #define _ITERATOR_DEBUG_LEVEL choose_your_value
#endif // WIN32 or _WIN64
// TODO: Windows DLL configurations have not yet been added to the project,
// but these defines are required to support them (when we do add them).
//
// Add GTE_EXPORT to project preprocessor options for dynamic library
// configurations to export their symbols.
#if defined(GTE_EXPORT)
// For the dynamic library configurations.
#define GTE_IMPEXP __declspec(dllexport)
#else
// For a client of the dynamic library or for the static library
// configurations.
#define GTE_IMPEXP
#endif
// Expose exactly one of these.
#define GTE_USE_ROW_MAJOR
//#define GTE_USE_COL_MAJOR
// Expose exactly one of these.
#define GTE_USE_MAT_VEC
//#define GTE_USE_VEC_MAT
#if (defined(GTE_USE_ROW_MAJOR) && defined(GTE_USE_COL_MAJOR)) || (!defined(GTE_USE_ROW_MAJOR) && !defined(GTE_USE_COL_MAJOR))
#error Exactly one storage order must be specified.
#endif
#if (defined(GTE_USE_MAT_VEC) && defined(GTE_USE_VEC_MAT)) || (!defined(GTE_USE_MAT_VEC) && !defined(GTE_USE_VEC_MAT))
#error Exactly one multiplication convention must be specified.
#endif
src/geometric-tools/Mathematics/GteDCPQuery.h
0 → 100644
View file @
2dbd2392
// David Eberly, Geometric Tools, Redmond WA 98052
// Copyright (c) 1998-2018
// Distributed under the Boost Software License, Version 1.0.
// http://www.boost.org/LICENSE_1_0.txt
// http://www.geometrictools.com/License/Boost/LICENSE_1_0.txt
// File Version: 3.0.1 (2018/10/05)
#pragma once
#ifndef GTE_DCPQUERY_H
#define GTE_DCPQUERY_H
#include
<Mathematics/GteMath.h>
namespace
gte
{
// Distance and closest-point queries.
template
<
typename
Real
,
typename
Type0
,
typename
Type1
>
class
DCPQuery
{
public:
struct
Result
{
// A DCPQuery-base class B must define a B::Result struct with member
// 'Real distance'. A DCPQuery-derived class D must also derive a
// D::Result from B:Result but may have no members. The idea is to
// allow Result to store closest-point information in addition to the
// distance. The operator() is non-const to allow DCPQuery to store
// and modify private state that supports the query.
};
Result
operator
()(
Type0
const
&
primitive0
,
Type1
const
&
primitive1
);
};
}
#endif
src/geometric-tools/Mathematics/GteDistLine3Triangle3.h
0 → 100644
View file @
2dbd2392
// David Eberly, Geometric Tools, Redmond WA 98052
// Copyright (c) 1998-2018
// Distributed under the Boost Software License, Version 1.0.
// http://www.boost.org/LICENSE_1_0.txt
// http://www.geometrictools.com/License/Boost/LICENSE_1_0.txt
// File Version: 3.0.0 (2016/06/19)
#pragma once
#ifndef GTE_DIST_LINE3_TRIANGLE3_H
#define GTE_DIST_LINE3_TRIANGLE3_H
#include
<Mathematics/GteVector3.h>
#include
<Mathematics/GteDistLineSegment.h>
#include
<Mathematics/GteTriangle.h>
#include
<limits>
namespace
gte
{
template
<
typename
Real
>
class
DCPQuery
<
Real
,
Line3
<
Real
>
,
Triangle3
<
Real
>>
{
public:
struct
Result
{
Real
distance
,
sqrDistance
;
Real
lineParameter
,
triangleParameter
[
3
];
Vector3
<
Real
>
closestPoint
[
2
];
};
Result
operator
()(
Line3
<
Real
>
const
&
line
,
Triangle3
<
Real
>
const
&
triangle
);
};
template
<
typename
Real
>
typename
DCPQuery
<
Real
,
Line3
<
Real
>
,
Triangle3
<
Real
>>::
Result
DCPQuery
<
Real
,
Line3
<
Real
>
,
Triangle3
<
Real
>>::
operator
()(
Line3
<
Real
>
const
&
line
,
Triangle3
<
Real
>
const
&
triangle
)
{
Result
result
;
// Test if line intersects triangle. If so, the squared distance is zero.
Vector3
<
Real
>
edge0
=
triangle
.
v
[
1
]
-
triangle
.
v
[
0
];
Vector3
<
Real
>
edge1
=
triangle
.
v
[
2
]
-
triangle
.
v
[
0
];
Vector3
<
Real
>
normal
=
UnitCross
(
edge0
,
edge1
);
Real
NdD
=
Dot
(
normal
,
line
.
direction
);
if
(
std
::
abs
(
NdD
)
>
(
Real
)
0
)
{
// The line and triangle are not parallel, so the line intersects
// the plane of the triangle.
Vector3
<
Real
>
diff
=
line
.
origin
-
triangle
.
v
[
0
];
Vector3
<
Real
>
basis
[
3
];
// {D, U, V}
basis
[
0
]
=
line
.
direction
;
ComputeOrthogonalComplement
<
Real
>
(
1
,
basis
);
Real
UdE0
=
Dot
(
basis
[
1
],
edge0
);
Real
UdE1
=
Dot
(
basis
[
1
],
edge1
);
Real
UdDiff
=
Dot
(
basis
[
1
],
diff
);
Real
VdE0
=
Dot
(
basis
[
2
],
edge0
);
Real
VdE1
=
Dot
(
basis
[
2
],
edge1
);
Real
VdDiff
=
Dot
(
basis
[
2
],
diff
);
Real
invDet
=
((
Real
)
1
)
/
(
UdE0
*
VdE1
-
UdE1
*
VdE0
);
// Barycentric coordinates for the point of intersection.
Real
b1
=
(
VdE1
*
UdDiff
-
UdE1
*
VdDiff
)
*
invDet
;
Real
b2
=
(
UdE0
*
VdDiff
-
VdE0
*
UdDiff
)
*
invDet
;
Real
b0
=
(
Real
)
1
-
b1
-
b2
;
if
(
b0
>=
(
Real
)
0
&&
b1
>=
(
Real
)
0
&&
b2
>=
(
Real
)
0
)
{
// Line parameter for the point of intersection.
Real
DdE0
=
Dot
(
line
.
direction
,
edge0
);
Real
DdE1
=
Dot
(
line
.
direction
,
edge1
);
Real
DdDiff
=
Dot
(
line
.
direction
,
diff
);
result
.
lineParameter
=
b1
*
DdE0
+
b2
*
DdE1
-
DdDiff
;
// Barycentric coordinates for the point of intersection.
result
.
triangleParameter
[
0
]
=
b0
;
result
.
triangleParameter
[
1
]
=
b1
;
result
.
triangleParameter
[
2
]
=
b2
;
// The intersection point is inside or on the triangle.
result
.
closestPoint
[
0
]
=
line
.
origin
+
result
.
lineParameter
*
line
.
direction
;
result
.
closestPoint
[
1
]
=
triangle
.
v
[
0
]
+
b1
*
edge0
+
b2
*
edge1
;
result
.
distance
=
(
Real
)
0
;
result
.
sqrDistance
=
(
Real
)
0
;
return
result
;
}
}
// Either (1) the line is not parallel to the triangle and the point of
// intersection of the line and the plane of the triangle is outside the
// triangle or (2) the line and triangle are parallel. Regardless, the
// closest point on the triangle is on an edge of the triangle. Compare
// the line to all three edges of the triangle.
result
.
distance
=
std
::
numeric_limits
<
Real
>::
max
();
result
.
sqrDistance
=
std
::
numeric_limits
<
Real
>::
max
();
for
(
int
i0
=
2
,
i1
=
0
;
i1
<
3
;
i0
=
i1
++
)
{
Vector3
<
Real
>
segCenter
=
((
Real
)
0.5
)
*
(
triangle
.
v
[
i0
]
+
triangle
.
v
[
i1
]);
Vector3
<
Real
>
segDirection
=
triangle
.
v
[
i1
]
-
triangle
.
v
[
i0
];
Real
segExtent
=
((
Real
)
0.5
)
*
Normalize
(
segDirection
);
Segment3
<
Real
>
segment
(
segCenter
,
segDirection
,
segExtent
);
DCPQuery
<
Real
,
Line3
<
Real
>
,
Segment3
<
Real
>>
query
;
auto
lsResult
=
query
(
line
,
segment
);
if
(
lsResult
.
sqrDistance
<
result
.
sqrDistance
)
{
result
.
sqrDistance
=
lsResult
.
sqrDistance
;
result
.
distance
=
lsResult
.
distance
;
result
.
lineParameter
=
lsResult
.
parameter
[
0
];
result
.
triangleParameter
[
i0
]
=
((
Real
)
0.5
)
*
((
Real
)
1
-
lsResult
.
parameter
[
0
]
/
segExtent
);
result
.
triangleParameter
[
i1
]
=
(
Real
)
1
-
result
.
triangleParameter
[
i0
];
result
.
triangleParameter
[
3
-
i0
-
i1
]
=
(
Real
)
0
;
result
.
closestPoint
[
0
]
=
lsResult
.
closestPoint
[
0
];
result
.
closestPoint
[
1
]
=
lsResult
.
closestPoint
[
1
];
}
}
return
result
;
}
}
#endif
src/geometric-tools/Mathematics/GteDistLineSegment.h
0 → 100644
View file @
2dbd2392
// David Eberly, Geometric Tools, Redmond WA 98052
// Copyright (c) 1998-2018
// Distributed under the Boost Software License, Version 1.0.
// http://www.boost.org/LICENSE_1_0.txt
// http://www.geometrictools.com/License/Boost/LICENSE_1_0.txt
// File Version: 3.0.1 (2018/10/05)
#pragma once
#ifndef GTE_DIST_LINE_SEGMENT_H
#define GTE_DIST_LINE_SEGMENT_H
#include
<Mathematics/GteDCPQuery.h>
#include
<Mathematics/GteLine.h>
#include
<Mathematics/GteSegment.h>
namespace
gte
{
template
<
int
N
,
typename
Real
>
class
DCPQuery
<
Real
,
Line
<
N
,
Real
>
,
Segment
<
N
,
Real
>>
{
public:
struct
Result
{
Real
distance
,
sqrDistance
;
Real
parameter
[
2
];
Vector
<
N
,
Real
>
closestPoint
[
2
];
};
// The centered form of the 'segment' is used. Thus, parameter[1] of
// the result is in [-e,e], where e = |segment.p[1] - segment.p[0]|/2.
Result
operator
()(
Line
<
N
,
Real
>
const
&
line
,
Segment
<
N
,
Real
>
const
&
segment
);
};
// Template aliases for convenience.
template
<
int
N
,
typename
Real
>
using
DCPLineSegment
=
DCPQuery
<
Real
,
Line
<
N
,
Real
>
,
Segment
<
N
,
Real
>>
;
template
<
typename
Real
>
using
DCPLine2Segment2
=
DCPLineSegment
<
2
,
Real
>
;
template
<
typename
Real
>
using
DCPLine3Segment3
=
DCPLineSegment
<
3
,
Real
>
;
template
<
int
N
,
typename
Real
>
typename
DCPQuery
<
Real
,
Line
<
N
,
Real
>
,
Segment
<
N
,
Real
>>::
Result
DCPQuery
<
Real
,
Line
<
N
,
Real
>
,
Segment
<
N
,
Real
>>::
operator
()(
Line
<
N
,
Real
>
const
&
line
,
Segment
<
N
,
Real
>
const
&
segment
)
{
Result
result
;
Vector
<
N
,
Real
>
segCenter
,
segDirection
;
Real
segExtent
;
segment
.
GetCenteredForm
(
segCenter
,
segDirection
,
segExtent
);
Vector
<
N
,
Real
>
diff
=
line
.
origin
-
segCenter
;
Real
a01
=
-
Dot
(
line
.
direction
,
segDirection
);
Real
b0
=
Dot
(
diff
,
line
.
direction
);
Real
s0
,
s1
;
if
(
std
::
abs
(
a01
)
<
(
Real
)
1
)
{
// The line and segment are not parallel.
Real
det
=
(
Real
)
1
-
a01
*
a01
;
Real
extDet
=
segExtent
*
det
;
Real
b1
=
-
Dot
(
diff
,
segDirection
);
s1
=
a01
*
b0
-
b1
;
if
(
s1
>=
-
extDet
)
{
if
(
s1
<=
extDet
)
{
// Two interior points are closest, one on the line and one
// on the segment.
s0
=
(
a01
*
b1
-
b0
)
/
det
;
s1
/=
det
;
}
else
{
// The endpoint e1 of the segment and an interior point of
// the line are closest.
s1
=
segExtent
;
s0
=
-
(
a01
*
s1
+
b0
);
}
}
else
{
// The endpoint e0 of the segment and an interior point of the
// line are closest.
s1
=
-
segExtent
;
s0
=
-
(
a01
*
s1
+
b0
);
}
}
else
{
// The line and segment are parallel. Choose the closest pair so that
// one point is at segment origin.
s1
=
(
Real
)
0
;
s0
=
-
b0
;
}
result
.
parameter
[
0
]
=
s0
;
result
.
parameter
[
1
]
=
s1
;
result
.
closestPoint
[
0
]
=
line
.
origin
+
s0
*
line
.
direction
;
result
.
closestPoint
[
1
]
=
segCenter
+
s1
*
segDirection
;
diff
=
result
.
closestPoint
[
0
]
-
result
.
closestPoint
[
1
];
result
.
sqrDistance
=
Dot
(
diff
,
diff
);
result
.
distance
=
std
::
sqrt
(
result
.
sqrDistance
);
return
result
;
}
}
#endif
src/geometric-tools/Mathematics/GteDistPointTriangle.h
0 → 100644
View file @
2dbd2392
// David Eberly, Geometric Tools, Redmond WA 98052
// Copyright (c) 1998-2018
// Distributed under the Boost Software License, Version 1.0.
// http://www.boost.org/LICENSE_1_0.txt
// http://www.geometrictools.com/License/Boost/LICENSE_1_0.txt
// File Version: 3.0.1 (2018/10/05)
#pragma once
#ifndef GTE_DIST_POINT_TRIANGLE_H
#define GTE_DIST_POINT_TRIANGLE_H
#include
<Mathematics/GteVector.h>
#include
<Mathematics/GteDCPQuery.h>
#include
<Mathematics/GteTriangle.h>
namespace
gte
{
template
<
int
N
,
typename
Real
>
class
DCPQuery
<
Real
,
Vector
<
N
,
Real
>
,
Triangle
<
N
,
Real
>>
{
public:
struct
Result
{
Real
distance
,
sqrDistance
;
Real
parameter
[
3
];
// barycentric coordinates for triangle.v[3]
Vector
<
N
,
Real
>
closest
;
};
Result
operator
()(
Vector
<
N
,
Real
>
const
&
point
,
Triangle
<
N
,
Real
>
const
&
triangle
);
private:
inline
void
GetMinEdge02
(
Real
const
&
a11
,
Real
const
&
b1
,
Vector
<
2
,
Real
>&
p
)
const
;
inline
void
GetMinEdge12
(
Real
const
&
a01
,
Real
const
&
a11
,
Real
const
&
b1
,
Real
const
&
f10
,
Real
const
&
f01
,
Vector
<
2
,
Real
>&
p
)
const
;
inline
void
GetMinInterior
(
Vector
<
2
,
Real
>
const
&
p0
,
Real
const
&
h0
,
Vector
<
2
,
Real
>
const
&
p1
,
Real
const
&
h1
,
Vector
<
2
,
Real
>&
p
)
const
;
};
// Template aliases for convenience.
template
<
int
N
,
typename
Real
>
using
DCPPointTriangle
=
DCPQuery
<
Real
,
Vector
<
N
,
Real
>
,
Triangle
<
N
,
Real
>>
;
template
<
typename
Real
>
using
DCPPoint2Triangle2
=
DCPPointTriangle
<
2
,
Real
>
;
template
<
typename
Real
>
using
DCPPoint3Triangle3
=
DCPPointTriangle
<
3
,
Real
>
;
template
<
int
N
,
typename
Real
>
inline
void
DCPQuery
<
Real
,
Vector
<
N
,
Real
>
,
Triangle
<
N
,
Real
>>::
GetMinEdge02
(
Real
const
&
a11
,
Real
const
&
b1
,
Vector
<
2
,
Real
>&
p
)
const
{
p
[
0
]
=
(
Real
)
0
;
if
(
b1
>=
(
Real
)
0
)
{
p
[
1
]
=
(
Real
)
0
;
}
else
if
(
a11
+
b1
<=
(
Real
)
0
)
{
p
[
1
]
=
(
Real
)
1
;
}
else
{
p
[
1
]
=
-
b1
/
a11
;
}
}
template
<
int
N
,
typename
Real
>
inline
void
DCPQuery
<
Real
,
Vector
<
N
,
Real
>
,
Triangle
<
N
,
Real
>>::
GetMinEdge12
(
Real
const
&
a01
,
Real
const
&
a11
,
Real
const
&
b1
,
Real
const
&
f10
,
Real
const
&
f01
,
Vector
<
2
,
Real
>&
p
)
const
{
Real
h0
=
a01
+
b1
-
f10
;
if
(
h0
>=
(
Real
)
0
)
{
p
[
1
]
=
(
Real
)
0
;
}
else
{
Real
h1
=
a11
+
b1
-
f01
;
if
(
h1
<=
(
Real
)
0
)
{
p
[
1
]
=
(
Real
)
1
;
}
else
{
p
[
1
]
=
h0
/
(
h0
-
h1
);
}
}
p
[
0
]
=
(
Real
)
1
-
p
[
1
];
}
template
<
int
N
,
typename
Real
>
inline
void
DCPQuery
<
Real
,
Vector
<
N
,
Real
>
,
Triangle
<
N
,
Real
>>::
GetMinInterior
(
Vector
<
2
,
Real
>
const
&
p0
,
Real
const
&
h0
,
Vector
<
2
,
Real
>
const
&
p1
,
Real
const
&
h1
,
Vector
<
2
,
Real
>&
p
)
const
{
Real
z
=
h0
/
(
h0
-
h1
);
p
=
((
Real
)
1
-
z
)
*
p0
+
z
*
p1
;
}
template
<
int
N
,
typename
Real
>
typename
DCPQuery
<
Real
,
Vector
<
N
,
Real
>
,
Triangle
<
N
,
Real
>>::
Result
DCPQuery
<
Real
,
Vector
<
N
,
Real
>
,
Triangle
<
N
,
Real
>>::
operator
()(
Vector
<
N
,
Real
>
const
&
point
,
Triangle
<
N
,
Real
>
const
&
triangle
)
{
Vector
<
N
,
Real
>
diff
=
point
-
triangle
.
v
[
0
];
Vector
<
N
,
Real
>
edge0
=
triangle
.
v
[
1
]
-
triangle
.
v
[
0
];
Vector
<
N
,
Real
>
edge1
=
triangle
.
v
[
2
]
-
triangle
.
v
[
0
];
Real
a00
=
Dot
(
edge0
,
edge0
);
Real
a01
=
Dot
(
edge0
,
edge1
);
Real
a11
=
Dot
(
edge1
,
edge1
);
Real
b0
=
-
Dot
(
diff
,
edge0
);
Real
b1
=
-
Dot
(
diff
,
edge1
);
Real
f00
=
b0
;
Real
f10
=
b0
+
a00
;
Real
f01
=
b0
+
a01
;
Vector
<
2
,
Real
>
p0
,
p1
,
p
;
Real
dt1
,
h0
,
h1
;
// Compute the endpoints p0 and p1 of the segment. The segment is
// parameterized by L(z) = (1-z)*p0 + z*p1 for z in [0,1] and the
// directional derivative of half the quadratic on the segment is
// H(z) = Dot(p1-p0,gradient[Q](L(z))/2), where gradient[Q]/2 = (F,G).
// By design, F(L(z)) = 0 for cases (2), (4), (5), and (6). Cases (1) and
// (3) can correspond to no-intersection or intersection of F = 0 with the
// triangle.
if
(
f00
>=
(
Real
)
0
)
{
if
(
f01
>=
(
Real
)
0
)
{
// (1) p0 = (0,0), p1 = (0,1), H(z) = G(L(z))
GetMinEdge02
(
a11
,
b1
,
p
);
}
else
{
// (2) p0 = (0,t10), p1 = (t01,1-t01), H(z) = (t11 - t10)*G(L(z))
p0
[
0
]
=
(
Real
)
0
;
p0
[
1
]
=
f00
/
(
f00
-
f01
);
p1
[
0
]
=
f01
/
(
f01
-
f10
);
p1
[
1
]
=
(
Real
)
1
-
p1
[
0
];
dt1
=
p1
[
1
]
-
p0
[
1
];
h0
=
dt1
*
(
a11
*
p0
[
1
]
+
b1
);
if
(
h0
>=
(
Real
)
0
)
{
GetMinEdge02
(
a11
,
b1
,
p
);
}
else
{
h1
=
dt1
*
(
a01
*
p1
[
0
]
+
a11
*
p1
[
1
]
+
b1
);
if
(
h1
<=
(
Real
)
0
)
{
GetMinEdge12
(
a01
,
a11
,
b1
,
f10
,
f01
,
p
);
}
else
{
GetMinInterior
(
p0
,
h0
,
p1
,
h1
,
p
);
}
}
}
}
else
if
(
f01
<=
(
Real
)
0
)
{
if
(
f10
<=
(
Real
)
0
)
{
// (3) p0 = (1,0), p1 = (0,1), H(z) = G(L(z)) - F(L(z))
GetMinEdge12
(
a01
,
a11
,
b1
,
f10
,
f01
,
p
);
}
else
{
// (4) p0 = (t00,0), p1 = (t01,1-t01), H(z) = t11*G(L(z))
p0
[
0
]
=
f00
/
(
f00
-
f10
);
p0
[
1
]
=
(
Real
)
0
;
p1
[
0
]
=
f01
/
(
f01
-
f10
);
p1
[
1
]
=
(
Real
)
1
-
p1
[
0
];
h0
=
p1
[
1
]
*
(
a01
*
p0
[
0
]
+
b1
);
if
(
h0
>=
(
Real
)
0
)
{
p
=
p0
;
// GetMinEdge01
}
else
{
h1
=
p1
[
1
]
*
(
a01
*
p1
[
0
]
+
a11
*
p1
[
1
]
+
b1
);
if
(
h1
<=
(
Real
)
0
)
{
GetMinEdge12
(
a01
,
a11
,
b1
,
f10
,
f01
,
p
);
}
else
{
GetMinInterior
(
p0
,
h0
,
p1
,
h1
,
p
);
}
}
}
}
else
if
(
f10
<=
(
Real
)
0
)
{
// (5) p0 = (0,t10), p1 = (t01,1-t01), H(z) = (t11 - t10)*G(L(z))
p0
[
0
]
=
(
Real
)
0
;
p0
[
1
]
=
f00
/
(
f00
-
f01
);
p1
[
0
]
=
f01
/
(
f01
-
f10
);
p1
[
1
]
=
(
Real
)
1
-
p1
[
0
];
dt1
=
p1
[
1
]
-
p0
[
1
];
h0
=
dt1
*
(
a11
*
p0
[
1
]
+
b1
);
if
(
h0
>=
(
Real
)
0
)
{
GetMinEdge02
(
a11
,
b1
,
p
);
}
else
{
h1
=
dt1
*
(
a01
*
p1
[
0
]
+
a11
*
p1
[
1
]
+
b1
);
if
(
h1
<=
(
Real
)
0
)
{