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
1bac302a
Commit
1bac302a
authored
Aug 29, 2019
by
Joseph Mirabel
Browse files
[GJK] Factorize some code.
parent
175a0a88
Changes
1
Hide whitespace changes
Inline
Side-by-side
src/narrowphase/gjk.cpp
View file @
1bac302a
...
...
@@ -573,43 +573,93 @@ bool GJK::encloseOrigin()
return
false
;
}
FCL_REAL
GJK
::
projectLineOrigin
(
const
Simplex
&
current
,
Simplex
&
next
)
inline
void
originToPoint
(
const
GJK
::
Simplex
&
current
,
int
a
,
const
Vec3f
&
A
,
GJK
::
Simplex
&
next
,
Vec3f
&
ray
)
{
const
int
a
=
1
,
b
=
0
;
// A is the last point we added.
const
Vec3f
&
A
=
current
.
vertex
[
a
]
->
w
;
const
Vec3f
&
B
=
current
.
vertex
[
b
]
->
w
;
const
Vec3f
AB
=
B
-
A
;
const
FCL_REAL
d
=
AB
.
dot
(
-
A
);
assert
(
d
<=
AB
.
squaredNorm
());
if
(
d
<=
0
)
{
// A is the closest to the origin
ray
=
A
;
next
.
vertex
[
0
]
=
current
.
vertex
[
a
];
next
.
rank
=
1
;
free_v
[
nfree
++
]
=
current
.
vertex
[
b
];
// To ensure backward compatibility
next
.
coefficient
[
0
]
=
1
;
return
A
.
squaredNorm
();
}
else
{
}
inline
void
originToSegment
(
const
GJK
::
Simplex
&
current
,
int
a
,
int
b
,
const
Vec3f
&
A
,
const
Vec3f
&
B
,
const
Vec3f
&
AB
,
const
FCL_REAL
&
ABdotAO
,
GJK
::
Simplex
&
next
,
Vec3f
&
ray
)
{
// ray = - ( AB ^ AO ) ^ AB = (AB.B) A + (-AB.A) B
ray
=
AB
.
dot
(
B
)
*
A
+
d
*
B
;
ray
=
AB
.
dot
(
B
)
*
A
+
ABdotAO
*
B
;
next
.
vertex
[
0
]
=
current
.
vertex
[
b
];
next
.
vertex
[
1
]
=
current
.
vertex
[
a
];
next
.
rank
=
2
;
// To ensure backward compatibility
FCL_REAL
alpha
=
d
/
AB
.
squaredNorm
();
FCL_REAL
alpha
=
ABdotAO
/
AB
.
squaredNorm
();
next
.
coefficient
[
1
]
=
1
-
alpha
;
// A
next
.
coefficient
[
0
]
=
alpha
;
// B
ray
/=
AB
.
squaredNorm
();
}
return
ray
.
squaredNorm
();
}
inline
void
originToTriangle
(
const
GJK
::
Simplex
&
current
,
int
a
,
int
b
,
int
c
,
const
Vec3f
&
A
,
const
Vec3f
&
AB
,
const
Vec3f
&
AC
,
const
Vec3f
&
ABC
,
GJK
::
Simplex
&
next
,
Vec3f
&
ray
)
{
ray
=
ABC
;
next
.
vertex
[
0
]
=
current
.
vertex
[
c
];
next
.
vertex
[
1
]
=
current
.
vertex
[
b
];
next
.
vertex
[
2
]
=
current
.
vertex
[
a
];
next
.
rank
=
3
;
// To ensure backward compatibility
FCL_REAL
s
=
ABC
.
squaredNorm
();
ray
*=
A
.
dot
(
ABC
)
/
s
;
Vec3f
AQ
(
ray
-
A
);
Vec3f
m
(
ABC
.
cross
(
AB
));
FCL_REAL
_u2
=
1
/
AB
.
squaredNorm
();
FCL_REAL
wu_u2
=
AQ
.
dot
(
AB
)
*
_u2
;
FCL_REAL
vu_u2
=
AC
.
dot
(
AB
)
*
_u2
;
FCL_REAL
wm_vm
=
AQ
.
dot
(
m
)
/
AC
.
dot
(
m
);
next
.
coefficient
[
0
]
=
wm_vm
;
// C
next
.
coefficient
[
1
]
=
wu_u2
-
wm_vm
*
vu_u2
;
// B
next
.
coefficient
[
2
]
=
1
-
next
.
coefficient
[
0
]
-
next
.
coefficient
[
1
];
// A
assert
(
0.
<=
next
.
coefficient
[
0
]
&&
next
.
coefficient
[
0
]
<=
1.
);
assert
(
0.
<=
next
.
coefficient
[
1
]
&&
next
.
coefficient
[
1
]
<=
1.
);
assert
(
0.
<=
next
.
coefficient
[
2
]
&&
next
.
coefficient
[
2
]
<=
1.
);
}
FCL_REAL
GJK
::
projectLineOrigin
(
const
Simplex
&
current
,
Simplex
&
next
)
{
const
int
a
=
1
,
b
=
0
;
// A is the last point we added.
const
Vec3f
&
A
=
current
.
vertex
[
a
]
->
w
;
const
Vec3f
&
B
=
current
.
vertex
[
b
]
->
w
;
const
Vec3f
AB
=
B
-
A
;
const
FCL_REAL
d
=
AB
.
dot
(
-
A
);
assert
(
d
<=
AB
.
squaredNorm
());
if
(
d
<=
0
)
{
// A is the closest to the origin
originToPoint
(
current
,
a
,
A
,
next
,
ray
);
free_v
[
nfree
++
]
=
current
.
vertex
[
b
];
}
else
originToSegment
(
current
,
a
,
b
,
A
,
B
,
AB
,
d
,
next
,
ray
);
return
ray
.
squaredNorm
();
}
FCL_REAL
GJK
::
projectTriangleOrigin
(
const
Simplex
&
current
,
Simplex
&
next
)
...
...
@@ -628,48 +678,18 @@ FCL_REAL GJK::projectTriangleOrigin(const Simplex& current, Simplex& next)
if
(
edgeAC2o
>=
0
)
{
FCL_REAL
towardsC
=
AC
.
dot
(
-
A
);
if
(
towardsC
>=
0
)
{
// Region 1
// ray = - ( AC ^ AO ) ^ AC = (AC.C) A + (-AC.A) C
ray
=
AC
.
dot
(
C
)
*
A
+
towardsC
*
C
;
next
.
vertex
[
0
]
=
current
.
vertex
[
c
];
next
.
vertex
[
1
]
=
current
.
vertex
[
a
];
next
.
rank
=
2
;
originToSegment
(
current
,
a
,
c
,
A
,
C
,
AC
,
towardsC
,
next
,
ray
);
free_v
[
nfree
++
]
=
current
.
vertex
[
b
];
// To ensure backward compatibility
FCL_REAL
alpha
=
towardsC
/
AC
.
squaredNorm
();
next
.
coefficient
[
1
]
=
1
-
alpha
;
// A
next
.
coefficient
[
0
]
=
alpha
;
// C
ray
/=
AC
.
squaredNorm
();
return
ray
.
squaredNorm
();
}
else
{
// Region 4 or 5
FCL_REAL
towardsB
=
AB
.
dot
(
-
A
);
if
(
towardsB
<
0
)
{
// Region 5
// A is the closest to the origin
ray
=
A
;
next
.
vertex
[
0
]
=
current
.
vertex
[
a
];
next
.
rank
=
1
;
originToPoint
(
current
,
a
,
A
,
next
,
ray
);
free_v
[
nfree
++
]
=
current
.
vertex
[
c
];
free_v
[
nfree
++
]
=
current
.
vertex
[
b
];
// To ensure backward compatibility
next
.
coefficient
[
0
]
=
1
;
return
A
.
squaredNorm
();
}
else
{
// Region 4
// ray = - ( AB ^ AO ) ^ AB = (AB.B) A + (-AB.A) B
ray
=
AB
.
dot
(
B
)
*
A
+
towardsB
*
B
;
next
.
vertex
[
0
]
=
current
.
vertex
[
b
];
next
.
vertex
[
1
]
=
current
.
vertex
[
a
];
next
.
rank
=
2
;
originToSegment
(
current
,
a
,
b
,
A
,
B
,
AB
,
towardsB
,
next
,
ray
);
free_v
[
nfree
++
]
=
current
.
vertex
[
c
];
// To ensure backward compatibility
FCL_REAL
alpha
=
towardsB
/
AB
.
squaredNorm
();
next
.
coefficient
[
1
]
=
1
-
alpha
;
// A
next
.
coefficient
[
0
]
=
alpha
;
// B
ray
/=
AB
.
squaredNorm
();
return
ray
.
squaredNorm
();
}
}
}
else
{
...
...
@@ -678,83 +698,28 @@ FCL_REAL GJK::projectTriangleOrigin(const Simplex& current, Simplex& next)
FCL_REAL
towardsB
=
AB
.
dot
(
-
A
);
if
(
towardsB
<
0
)
{
// Region 5
// A is the closest to the origin
ray
=
A
;
next
.
vertex
[
0
]
=
current
.
vertex
[
a
];
next
.
rank
=
1
;
originToPoint
(
current
,
a
,
A
,
next
,
ray
);
free_v
[
nfree
++
]
=
current
.
vertex
[
c
];
free_v
[
nfree
++
]
=
current
.
vertex
[
b
];
// To ensure backward compatibility
next
.
coefficient
[
0
]
=
1
;
return
A
.
squaredNorm
();
}
else
{
// Region 4
// ray = - ( AB ^ AO ) ^ AB = (AB.B) A + (-AB.A) B
ray
=
AB
.
dot
(
B
)
*
A
+
towardsB
*
B
;
next
.
vertex
[
0
]
=
current
.
vertex
[
b
];
next
.
vertex
[
1
]
=
current
.
vertex
[
a
];
next
.
rank
=
2
;
originToSegment
(
current
,
a
,
b
,
A
,
B
,
AB
,
towardsB
,
next
,
ray
);
free_v
[
nfree
++
]
=
current
.
vertex
[
c
];
// To ensure backward compatibility
FCL_REAL
alpha
=
towardsB
/
AB
.
squaredNorm
();
next
.
coefficient
[
1
]
=
1
-
alpha
;
// A
next
.
coefficient
[
0
]
=
alpha
;
// B
ray
/=
AB
.
squaredNorm
();
return
ray
.
squaredNorm
();
}
}
else
{
// I don't think there is any need to check whether we are above or below
// the triangle.
originToTriangle
(
current
,
a
,
b
,
c
,
A
,
AB
,
AC
,
ABC
,
next
,
ray
);
/*
FCL_REAL aboveTri = ABC.dot(-A);
if (aboveTri) { // Region 2
ray
=
ABC
;
next
.
vertex
[
0
]
=
current
.
vertex
[
c
];
next
.
vertex
[
1
]
=
current
.
vertex
[
b
];
next
.
vertex
[
2
]
=
current
.
vertex
[
a
];
next
.
rank
=
3
;
// To ensure backward compatibility
FCL_REAL
s
=
ABC
.
squaredNorm
();
ray
*=
A
.
dot
(
ABC
)
/
s
;
Vec3f
AQ
(
ray
-
A
);
Vec3f
m
(
ABC
.
cross
(
AB
));
FCL_REAL
_u2
=
1
/
AB
.
squaredNorm
();
FCL_REAL
wu_u2
=
AQ
.
dot
(
AB
)
*
_u2
;
FCL_REAL
vu_u2
=
AC
.
dot
(
AB
)
*
_u2
;
FCL_REAL
wm_vm
=
AQ
.
dot
(
m
)
/
AC
.
dot
(
m
);
next
.
coefficient
[
0
]
=
wm_vm
;
// C
next
.
coefficient
[
1
]
=
wu_u2
-
wm_vm
*
vu_u2
;
// B
next
.
coefficient
[
2
]
=
1
-
next
.
coefficient
[
0
]
-
next
.
coefficient
[
1
];
// A
assert
(
0.
<=
next
.
coefficient
[
0
]
&&
next
.
coefficient
[
0
]
<=
1.
);
assert
(
0.
<=
next
.
coefficient
[
1
]
&&
next
.
coefficient
[
1
]
<=
1.
);
assert
(
0.
<=
next
.
coefficient
[
2
]
&&
next
.
coefficient
[
2
]
<=
1.
);
return
ray
.
squaredNorm
();
originToTriangle (current, a, b, c, A, AB, AC, ABC, next, ray);
} else { // Region 3
ray
=
ABC
;
//next.vertex[0] = current.vertex[b];
next
.
vertex
[
0
]
=
current
.
vertex
[
c
];
next
.
vertex
[
1
]
=
current
.
vertex
[
b
];
next
.
vertex
[
2
]
=
current
.
vertex
[
a
];
next
.
rank
=
3
;
// To ensure backward compatibility
FCL_REAL
s
=
ABC
.
squaredNorm
();
ray
*=
A
.
dot
(
ABC
)
/
s
;
next
.
coefficient
[
2
]
=
-
1
;
next
.
coefficient
[
1
]
=
-
1
;
next
.
coefficient
[
0
]
=
-
1
;
assert
(
0.
<=
next
.
coefficient
[
0
]
&&
next
.
coefficient
[
0
]
<=
1.
);
assert
(
0.
<=
next
.
coefficient
[
1
]
&&
next
.
coefficient
[
1
]
<=
1.
);
assert
(
0.
<=
next
.
coefficient
[
2
]
&&
next
.
coefficient
[
2
]
<=
1.
);
return
ray
.
squaredNorm
();
// Flip the ray
}
*/
}
}
return
ray
.
squaredNorm
();
}
void
EPA
::
initialize
()
...
...
Write
Preview
Supports
Markdown
0%
Try again
or
attach a new file
.
Cancel
You are about to add
0
people
to the discussion. Proceed with caution.
Finish editing this message first!
Cancel
Please
register
or
sign in
to comment