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
soth
Commits
5adbad1b
Commit
5adbad1b
authored
Jan 11, 2016
by
Olivier Stasse
Browse files
Switch Givens, BasicStage to Index type deriverd from MatrixXd::Index
parent
c10be584
Changes
14
Hide whitespace changes
Inline
Side-by-side
src/ActiveSet.hpp
View file @
5adbad1b
...
...
@@ -273,9 +273,12 @@ namespace soth
public:
inline
operator
Matrix
<
Index
,
Dynamic
,
1
>
(
void
)
const
{
return
getIndirection
();
}
SOTH_EXPORT
friend
std
::
ostream
&
operator
<<
(
std
::
ostream
&
os
,
const
ActiveSet
&
as
);
//
SOTH_EXPORT friend std::ostream& operator<< ( std::ostream & os,const ActiveSet
<typename Indirect>
& as );
};
template
<
typename
Indirect
>
SOTH_EXPORT
std
::
ostream
&
operator
<<
(
std
::
ostream
&
os
,
const
ActiveSet
<
Indirect
>&
as
);
/* The previous class is not aware of the indirection built upon WMLY. This indirection
* is added in this derivation, to make it transparent to the user.
...
...
@@ -492,7 +495,7 @@ namespace soth
getIndirection
(
void
)
const
{
if
(
nba
==
0
)
return
Matrix
<
Indirect
::
Index
,
Dynamic
,
1
>
();
return
Matrix
<
typename
Indirect
::
Index
,
Dynamic
,
1
>
();
Matrix
<
typename
Indirect
::
Index
,
Dynamic
,
1
>
res
(
nba
);
for
(
unsigned
int
r
=
0
;
r
<
nba
;
++
r
)
...
...
src/Algebra.hpp
View file @
5adbad1b
...
...
@@ -37,7 +37,7 @@ namespace soth
template
<
typename
Derived
>
MATLAB
(
const
MatrixBase
<
Derived
>
&
m1
,
bool
id
);
template
<
typename
Rotation
>
MATLAB
(
unsigned
int
size
,
const
Rotation
&
m1
);
MATLAB
(
const
MatrixXd
::
Index
size
,
const
Rotation
&
m1
);
template
<
typename
Derived
>
void
genericInit
(
const
MatrixBase
<
Derived
>
&
m1
);
...
...
src/Allocator.cpp
View file @
5adbad1b
...
...
@@ -36,7 +36,7 @@ namespace soth
put
(
const
Index
&
token
)
{
resource
.
push_front
(
token
);
assert
(
resource
.
size
()
<=
max
);
assert
(
(
Index
)
resource
.
size
()
<=
max
);
}
void
AllocatorML
::
...
...
src/BaseY.cpp
View file @
5adbad1b
...
...
@@ -3,7 +3,7 @@
namespace
soth
{
// Empty construction with memory allocation.
BaseY
::
BaseY
(
const
unsigned
int
&
insize
)
BaseY
::
BaseY
(
const
Index
&
insize
)
:
isExplicit
(
false
)
,
size
(
insize
)
,
rank
(
0
)
...
...
src/BaseY.hpp
View file @
5adbad1b
...
...
@@ -28,7 +28,7 @@ namespace soth
public:
// Empty construction with memory allocation.
BaseY
(
const
unsigned
int
&
size
);
BaseY
(
const
Index
&
size
);
void
computeExplicitly
();
void
reset
()
{
...
...
src/BasicStage.hpp
View file @
5adbad1b
...
...
@@ -8,10 +8,20 @@
#include
"soth/Bound.hpp"
#include
"soth/Algebra.hpp"
#include
<boost/noncopyable.hpp>
#include
<boost/version.hpp>
#ifndef WITHOUT_NOTIFIOR
#if BOOST_VERSION > 105300
#include
<boost/signals2.hpp>
#define NOTIFIOR_SIGNAL2
#else
#include
<boost/signals.hpp>
#endif
#endif
namespace
soth
{
...
...
@@ -73,7 +83,11 @@ namespace soth
public:
/* Notification, could be removed conditionnaly to the lack of boost::signal. */
#ifndef WITHOUT_NOTIFIOR
typedef
boost
::
function
<
void
(
std
::
string
,
ConstraintRef
,
std
::
string
)
>
listener_function_t
;
#ifdef NOTIFIOR_SIGNAL2
boost
::
signals2
::
signal
<
void
(
std
::
string
,
ConstraintRef
,
std
::
string
)
>
notifior
;
#else
boost
::
signal
<
void
(
std
::
string
,
ConstraintRef
,
std
::
string
)
>
notifior
;
#endif
#else
inline
void
notifior
(
int
,
int
,
std
::
string
)
{}
#endif
...
...
src/Bound.hpp
View file @
5adbad1b
...
...
@@ -67,6 +67,7 @@ namespace soth
};
extern
const
ConstraintRef
CONSTRAINT_VOID
;
typedef
std
::
vector
<
ConstraintRef
>
cstref_vector_t
;
typedef
std
::
vector
<
ConstraintRef
>::
size_type
cstref_vector_size_t
;
SOTH_EXPORT
std
::
ostream
&
operator
<<
(
std
::
ostream
&
os
,
const
VectorBound
&
t
);
SOTH_EXPORT
std
::
ostream
&
operator
<<
(
std
::
ostream
&
os
,
const
ConstraintRef
&
cst
);
...
...
src/Givens.cpp
View file @
5adbad1b
...
...
@@ -7,13 +7,13 @@ namespace soth
{
}
Givens
::
Givens
(
double
a
,
double
b
,
int
i
,
int
j
,
double
*
z
)
Givens
::
Givens
(
double
a
,
double
b
,
Index
i
,
Index
j
,
double
*
z
)
:
i
(
i
),
j
(
j
)
{
makeGivens
(
a
,
b
,
i
,
j
,
z
);
}
void
Givens
::
makeGivens
(
double
a
,
double
b
,
int
i
,
int
j
,
double
*
z
)
void
Givens
::
makeGivens
(
double
a
,
double
b
,
Index
i
,
Index
j
,
double
*
z
)
{
G
.
makeGivens
(
a
,
b
,
z
);
this
->
i
=
i
;
...
...
src/Givens.hpp
View file @
5adbad1b
...
...
@@ -22,25 +22,25 @@ namespace soth
{
public:
typedef
JacobiRotation
<
double
>
NestedType
;
typedef
MatrixXd
::
Index
Index
;
public:
SOTH_EXPORT
Givens
();
SOTH_EXPORT
Givens
(
double
a
,
double
b
,
int
i
,
int
j
,
double
*
z
=
0
);
SOTH_EXPORT
Givens
(
double
a
,
double
b
,
Index
i
,
Index
j
,
double
*
z
=
0
);
template
<
typename
VectorBase
>
Givens
(
const
VectorBase
&
v
,
int
i
,
int
j
);
Givens
(
const
VectorBase
&
v
,
Index
i
,
Index
j
);
template
<
typename
VectorBase
>
Givens
(
VectorBase
&
v
,
int
i
,
int
j
,
bool
apply
=
false
);
Givens
(
VectorBase
&
v
,
Index
i
,
Index
j
,
bool
apply
=
false
);
public:
SOTH_EXPORT
void
makeGivens
(
double
a
,
double
b
,
int
i
,
int
j
,
double
*
z
=
0
);
SOTH_EXPORT
void
makeGivens
(
double
a
,
double
b
,
Index
i
,
Index
j
,
double
*
z
=
0
);
/* Givens to the right, ie such that G'*v (:= G.applyTransposeOnTheRight(v) )
* is nullified. */
template
<
typename
VectorBase
>
void
makeGivens
(
const
VectorBase
&
v
,
int
i
,
int
j
);
void
makeGivens
(
const
VectorBase
&
v
,
Index
i
,
Index
j
);
template
<
typename
VectorBase
>
void
makeGivens
(
VectorBase
&
v
,
int
i
,
int
j
,
bool
apply
);
void
makeGivens
(
VectorBase
&
v
,
Index
i
,
Index
j
,
bool
apply
);
/* --- Multiplication --- */
// M := M*G.
...
...
@@ -68,15 +68,15 @@ namespace soth
void
applyThisOnTheLeftPartiel
(
SubMatrix
<
MatrixType
,
PermutationType
,
IsSub
>
&
M
)
const
;
// M := G'*M. -- TODO: use Index instead of int
template
<
typename
D
>
void
applyTransposeOnTheRight
(
MatrixBase
<
D
>
&
M
,
const
int
size
)
const
;
template
<
typename
D
>
void
applyTransposeOnTheRight
(
MatrixBase
<
D
>
&
M
,
const
Index
size
)
const
;
template
<
typename
MatrixType
,
int
PermutationType
,
bool
IsSub
>
void
applyTransposeOnTheRight
(
SubMatrix
<
MatrixType
,
PermutationType
,
IsSub
>
&
M
,
const
int
size
)
const
;
void
applyTransposeOnTheRight
(
SubMatrix
<
MatrixType
,
PermutationType
,
IsSub
>
&
M
,
const
Index
size
)
const
;
// TODO: same partiel method for all the cases
/* For application on lines of triangular matrices: check
* if the plannar rotation apply on the 0s. */
bool
applicable
(
unsigned
int
size
)
const
bool
applicable
(
Index
size
)
const
{
bool
ci
=
(
i
<
size
);
bool
cj
=
(
j
<
size
);
...
...
@@ -120,8 +120,8 @@ namespace soth
private:
public:
//DEBUG
NestedType
G
,
Gt
;
unsigned
int
i
;
unsigned
int
j
;
Index
i
;
Index
j
;
public:
// Transpose
...
...
@@ -243,14 +243,14 @@ namespace soth
/* --- Construction single ------------------------------------------------ */
template
<
typename
VectorBase
>
inline
Givens
::
Givens
(
const
VectorBase
&
v
,
int
i
,
int
j
)
inline
Givens
::
Givens
(
const
VectorBase
&
v
,
Index
i
,
Index
j
)
:
i
(
i
),
j
(
j
)
{
EIGEN_STATIC_ASSERT_VECTOR_ONLY
(
VectorBase
);
makeGivens
(
v
,
i
,
j
);
}
template
<
typename
VectorBase
>
inline
Givens
::
Givens
(
VectorBase
&
v
,
int
i
,
int
j
,
bool
apply
)
inline
Givens
::
Givens
(
VectorBase
&
v
,
Index
i
,
Index
j
,
bool
apply
)
:
i
(
i
),
j
(
j
)
{
EIGEN_STATIC_ASSERT_VECTOR_ONLY
(
VectorBase
);
...
...
@@ -258,14 +258,14 @@ namespace soth
}
template
<
typename
VectorBase
>
inline
void
Givens
::
makeGivens
(
const
VectorBase
&
v
,
int
i
,
int
j
)
inline
void
Givens
::
makeGivens
(
const
VectorBase
&
v
,
Index
i
,
Index
j
)
{
EIGEN_STATIC_ASSERT_VECTOR_ONLY
(
VectorBase
);
makeGivens
(
v
(
i
),
v
(
j
),
i
,
j
,
NULL
);
}
template
<
typename
VectorBase
>
inline
void
Givens
::
makeGivens
(
VectorBase
&
v
,
int
i
,
int
j
,
bool
apply
)
inline
void
Givens
::
makeGivens
(
VectorBase
&
v
,
Index
i
,
Index
j
,
bool
apply
)
{
EIGEN_STATIC_ASSERT_VECTOR_ONLY
(
VectorBase
);
if
(
!
apply
)
makeGivens
(
v
,
i
,
j
);
...
...
@@ -294,10 +294,10 @@ namespace soth
void
Givens
::
applyThisOnTheLeftPartiel
(
MatrixBase
<
Derived
>
&
M
)
const
{
if
(
(
int
(
i
)
<
M
.
cols
())
&&
(
int
(
j
)
<
M
.
cols
())
)
if
(
(
Index
(
i
)
<
M
.
cols
())
&&
(
Index
(
j
)
<
M
.
cols
())
)
M
.
applyOnTheRight
(
i
,
j
,
G
);
else
if
(
int
(
i
)
<
M
.
cols
())
{
assert
(
M
.
col
(
i
).
norm
()
<
1e-6
);
}
else
if
(
int
(
j
)
<
M
.
cols
())
{
assert
(
M
.
col
(
j
).
norm
()
<
1e-6
);
}
else
if
(
Index
(
i
)
<
M
.
cols
())
{
assert
(
M
.
col
(
i
).
norm
()
<
1e-6
);
}
else
if
(
Index
(
j
)
<
M
.
cols
())
{
assert
(
M
.
col
(
j
).
norm
()
<
1e-6
);
}
}
// M := M*G'.
...
...
@@ -366,7 +366,7 @@ namespace soth
void
Givens
::
applyThisOnTheLeft
(
SubMatrix
<
MatrixType
,
PermutationType
,
IsSub
>
&
M
)
const
{
assert
(
0
<=
i
&&
int
(
i
)
<
M
.
cols
()
&&
0
<=
j
&&
int
(
j
)
<
M
.
cols
()
);
assert
(
0
<=
i
&&
Index
(
i
)
<
M
.
cols
()
&&
0
<=
j
&&
Index
(
j
)
<
M
.
cols
()
);
typedef
typename
MatrixType
::
Index
Index
;
for
(
Index
r
=
0
;
r
<
M
.
rows
();
++
r
)
{
...
...
@@ -399,7 +399,7 @@ namespace soth
void
Givens
::
applyTransposeOnTheRight
(
SubMatrix
<
MatrixType
,
PermutationType
,
IsSub
>&
M
)
const
{
assert
(
0
<=
i
&&
int
(
i
)
<
M
.
rows
()
&&
0
<=
j
&&
int
(
j
)
<
M
.
rows
()
);
assert
(
0
<=
i
&&
Index
(
i
)
<
M
.
rows
()
&&
0
<=
j
&&
Index
(
j
)
<
M
.
rows
()
);
typedef
typename
MatrixType
::
Index
Index
;
for
(
Index
c
=
0
;
c
<
M
.
cols
();
++
c
)
{
...
...
@@ -441,9 +441,9 @@ namespace soth
template
<
typename
MatrixType
,
int
PermutationType
,
bool
IsSub
>
void
Givens
::
applyTransposeOnTheRight
(
SubMatrix
<
MatrixType
,
PermutationType
,
IsSub
>&
M
,
const
int
nbCols
)
const
const
Index
nbCols
)
const
{
assert
(
0
<=
i
&&
int
(
i
)
<
M
.
rows
()
&&
0
<=
j
&&
int
(
j
)
<
M
.
rows
()
);
assert
(
0
<=
i
&&
Index
(
i
)
<
M
.
rows
()
&&
0
<=
j
&&
Index
(
j
)
<
M
.
rows
()
);
assert
(
0
<=
nbCols
&&
nbCols
<=
M
.
cols
()
);
typedef
typename
MatrixType
::
Index
Index
;
for
(
Index
c
=
0
;
c
<
nbCols
;
++
c
)
...
...
@@ -483,7 +483,8 @@ namespace soth
void
GivensSequence
::
applyTransposeOnTheLeft
(
MatrixBase
<
Derived
>
&
M
)
const
{
for
(
int
i
=
G
.
size
()
-
1
;
i
>=
0
;
--
i
)
typedef
typename
MatrixBase
<
Derived
>::
Index
Index
;
for
(
Index
i
=
G
.
size
()
-
1
;
i
>=
0
;
--
i
)
G
[
i
].
applyTransposeOnTheLeft
(
M
);
}
...
...
@@ -492,7 +493,8 @@ namespace soth
void
GivensSequence
::
applyThisOnTheRight
(
MatrixBase
<
Derived
>
&
M
)
const
{
for
(
int
i
=
G
.
size
()
-
1
;
i
>=
0
;
--
i
)
typedef
typename
MatrixBase
<
Derived
>::
Index
Index
;
for
(
Index
i
=
G
.
size
()
-
1
;
i
>=
0
;
--
i
)
{
G
[
i
].
applyThisOnTheRight
(
M
);
}
}
...
...
@@ -509,7 +511,7 @@ namespace soth
/* --- Multiplication display -------------------------------------------- */
template
<
>
inline
MATLAB
::
MATLAB
(
unsigned
int
size
,
const
GivensSequence
&
m1
)
MATLAB
(
MatrixXd
::
Index
size
,
const
GivensSequence
&
m1
)
{
if
(
size
==
0
)
initMatrixColNull
(
0
);
else
...
...
src/HCOD.cpp
View file @
5adbad1b
...
...
@@ -11,7 +11,7 @@ namespace soth
{
HCOD
::
HCOD
(
unsigned
int
inSizeProblem
,
unsigned
int
nbStage
)
HCOD
(
Index
inSizeProblem
,
Index
nbStage
)
:
sizeProblem
(
inSizeProblem
)
,
Y
(
sizeProblem
)
...
...
@@ -39,13 +39,13 @@ namespace soth
isInit
=
false
;
}
void
HCOD
::
pushBackStage
(
const
unsigned
int
&
nr
,
const
double
*
Jdata
,
const
Bound
*
bdata
)
pushBackStage
(
const
Index
&
nr
,
const
double
*
Jdata
,
const
Bound
*
bdata
)
{
stages
.
push_back
(
stage_ptr_t
(
new
soth
::
Stage
(
nr
,
sizeProblem
,
Jdata
,
bdata
,
Y
)));
isInit
=
false
;
}
void
HCOD
::
pushBackStage
(
const
unsigned
int
&
nr
,
const
double
*
Jdata
)
pushBackStage
(
const
Index
&
nr
,
const
double
*
Jdata
)
{
stages
.
push_back
(
stage_ptr_t
(
new
soth
::
Stage
(
nr
,
sizeProblem
,
Jdata
,
Y
))
);
...
...
@@ -57,20 +57,20 @@ namespace soth
const
std
::
vector
<
VectorBound
>
&
bounds
)
{
assert
(
J
.
size
()
==
bounds
.
size
()
);
for
(
unsigned
int
i
=
0
;
i
<
J
.
size
();
++
i
)
for
(
std
::
vector
<
MatrixXd
>::
size_type
i
=
0
;
i
<
J
.
size
();
++
i
)
{
pushBackStage
(
J
[
i
],
bounds
[
i
]
);
}
}
Stage
&
HCOD
::
stage
(
unsigned
int
i
)
stage
(
Index
i
)
{
assert
(
i
<
stages
.
size
()
);
return
*
stages
[
i
];
}
const
Stage
&
HCOD
::
stage
(
unsigned
int
i
)
const
stage
(
Index
i
)
const
{
assert
(
i
<
stages
.
size
()
);
return
*
stages
[
i
];
...
...
@@ -86,14 +86,14 @@ namespace soth
}
void
HCOD
::
setInitialActiveSet
(
const
cstref_vector_t
&
Ir0
,
unsigned
int
k
)
setInitialActiveSet
(
const
cstref_vector_t
&
Ir0
,
Index
k
)
{
sotDEBUG
(
5
)
<<
"Ir["
<<
k
<<
"]"
<<
std
::
endl
;
stage
(
k
).
setInitialActiveSet
(
Ir0
,
true
);
}
cstref_vector_t
HCOD
::
getOptimalActiveSet
(
unsigned
int
k
)
getOptimalActiveSet
(
Index
k
)
{
return
stage
(
k
).
getOptimalActiveSet
();
}
...
...
@@ -199,7 +199,7 @@ namespace soth
assert
(
isReset
&&
(
!
isInit
)
);
sotDEBUG
(
5
)
<<
"Y during initialize:"
<<
Y
.
matrixExplicit
<<
std
::
endl
;
/* Compute the initial COD for each stage. */
for
(
unsigned
in
t
i
=
0
;
i
<
stages
.
size
();
++
i
)
for
(
stage_sequence_size_
t
i
=
0
;
i
<
stages
.
size
();
++
i
)
{
assert
(
stages
[
i
]
!=
0
);
sotDEBUG
(
5
)
<<
" --- STAGE "
<<
i
...
...
@@ -212,12 +212,12 @@ namespace soth
isReset
=
false
;
isInit
=
true
;
}
void
HCOD
::
update
(
const
unsigned
int
&
stageUp
,
const
ConstraintRef
&
cst
)
update
(
const
Index
&
stageUp
,
const
ConstraintRef
&
cst
)
{
assert
(
isInit
);
GivensSequence
Yup
;
unsigned
int
rankDef
=
stages
[
stageUp
]
->
update
(
cst
,
Yup
);
for
(
unsigned
in
t
i
=
stageUp
+
1
;
i
<
stages
.
size
();
++
i
)
Index
rankDef
=
stages
[
stageUp
]
->
update
(
cst
,
Yup
);
for
(
stage_sequence_size_
t
i
=
stageUp
+
1
;
i
<
stages
.
size
();
++
i
)
{
stages
[
i
]
->
propagateUpdate
(
Yup
,
rankDef
);
}
...
...
@@ -230,7 +230,7 @@ namespace soth
sotDEBUG
(
5
)
<<
"Update "
<<
(
*
stageIter
)
->
name
<<
", "
<<
cst
<<
std
::
endl
;
GivensSequence
Yup
;
unsigned
int
rankDef
=
(
*
stageIter
)
->
update
(
cst
,
Yup
);
Index
rankDef
=
(
*
stageIter
)
->
update
(
cst
,
Yup
);
for
(
++
stageIter
;
stageIter
!=
stages
.
end
();
++
stageIter
)
{
sotDEBUG
(
5
)
<<
"Y (updateY): "
<<
(
*
stageIter
)
->
name
<<
","
...
...
@@ -243,19 +243,19 @@ namespace soth
updateY
(
Yup
);
}
void
HCOD
::
downdate
(
const
unsigned
int
&
stageDown
,
const
unsigned
int
&
rowDown
)
downdate
(
const
Index
&
stageDown
,
const
Index
&
rowDown
)
{
assert
(
isInit
);
GivensSequence
Ydown
;
bool
propag
=
stages
[
stageDown
]
->
downdate
(
rowDown
,
Ydown
);
for
(
unsigned
in
t
i
=
stageDown
+
1
;
i
<
stages
.
size
();
++
i
)
for
(
stage_sequence_size_
t
i
=
stageDown
+
1
;
i
<
stages
.
size
();
++
i
)
{
propag
=
stages
[
i
]
->
propagateDowndate
(
Ydown
,
propag
);
}
updateY
(
Ydown
);
}
void
HCOD
::
downdate
(
stage_iter_t
stageIter
,
const
unsigned
int
&
rowDown
)
downdate
(
stage_iter_t
stageIter
,
const
Index
&
rowDown
)
{
assert
(
isInit
);
...
...
@@ -303,7 +303,7 @@ namespace soth
* The result is stored in the stages (getLagrangeMultipliers()).
*/
void
HCOD
::
computeLagrangeMultipliers
(
const
unsigned
int
&
stageRef
)
computeLagrangeMultipliers
(
const
Index
&
stageRef
)
{
assert
(
isSolutionCpt
);
assert
(
stageRef
<=
stages
.
size
()
);
...
...
@@ -334,7 +334,7 @@ namespace soth
sotDEBUG
(
5
)
<<
"Y= "
<<
(
MATLAB
)
Y
.
matrixExplicit
<<
std
::
endl
;
YtuNext
.
setZero
();
sotDEBUG
(
5
)
<<
"Y= "
<<
(
MATLAB
)
Y
.
matrixExplicit
<<
std
::
endl
;
for
(
unsigned
in
t
i
=
0
;
i
<
stages
.
size
();
++
i
)
for
(
stage_sequence_size_
t
i
=
0
;
i
<
stages
.
size
();
++
i
)
{
stages
[
i
]
->
computeSolution
(
YtuNext
);
}
...
...
@@ -418,12 +418,12 @@ namespace soth
/* Return true iff the search is positive, ie if downdate was
* needed and performed. */
bool
HCOD
::
searchAndDowndate
(
const
unsigned
int
&
stageRef
)
searchAndDowndate
(
const
Index
&
stageRef
)
{
assert
(
stageRef
<=
stages
.
size
()
);
double
lambdamax
=
Stage
::
EPSILON
;
unsigned
int
row
;
double
lambdamax
=
Stage
::
EPSILON
;
Index
row
;
const
stage_iter_t
refend
=
stages
.
begin
()
+
std
::
min
((
unsigned
)(
stages
.
size
()),
stageRef
+
1
);
const
stage_iter_t
refend
=
stages
.
begin
()
+
std
::
min
((
Index
)(
stages
.
size
()),
stageRef
+
1
);
stage_iter_t
stageDown
=
refend
;
for
(
stage_iter_t
iter
=
stages
.
begin
();
iter
!=
refend
;
++
iter
)
{
...
...
@@ -439,11 +439,11 @@ namespace soth
}
bool
HCOD
::
search
(
const
unsigned
int
&
stageRef
)
search
(
const
Index
&
stageRef
)
{
assert
(
stageRef
<=
stages
.
size
()
);
double
lambdamax
=
Stage
::
EPSILON
;
unsigned
int
row
;
const
stage_iter_t
refend
=
stages
.
begin
()
+
std
::
min
((
unsigned
)(
stages
.
size
()),
stageRef
+
1
);
double
lambdamax
=
Stage
::
EPSILON
;
Index
row
;
const
stage_iter_t
refend
=
stages
.
begin
()
+
std
::
min
((
Index
)(
stages
.
size
()),
stageRef
+
1
);
stage_iter_t
stageDown
=
refend
;
for
(
stage_iter_t
iter
=
stages
.
begin
();
iter
!=
refend
;
++
iter
)
{
...
...
@@ -530,7 +530,7 @@ namespace soth
time1 = ((t1.tv_sec-t0.tv_sec)+(t1.tv_usec-t0.tv_usec)/1.0e6);*/
int
iter
=
0
;
unsigned
int
stageMinimal
=
0
;
Index
stageMinimal
=
0
;
do
{
iter
++
;
sotDEBUG
(
5
)
<<
" --- ***
\t
"
<<
iter
<<
"
\t
***.---"
<<
std
::
endl
;
...
...
@@ -553,7 +553,7 @@ namespace soth
sotDEBUG
(
5
)
<<
"No update, make step ==1."
<<
std
::
endl
;
makeStep
();
for
(
;
stageMinimal
<=
stages
.
size
();
++
stageMinimal
)
for
(
;
stageMinimal
<=
(
Index
)
stages
.
size
();
++
stageMinimal
)
{
sotDEBUG
(
5
)
<<
"--- Started to examinate stage "
<<
stageMinimal
<<
std
::
endl
;
computeLagrangeMultipliers
(
stageMinimal
);
...
...
@@ -566,7 +566,7 @@ namespace soth
break
;
}
for
(
unsigned
int
i
=
0
;
i
<
stageMinimal
;
++
i
)
for
(
Index
i
=
0
;
i
<
stageMinimal
;
++
i
)
stages
[
i
]
->
freezeSlacks
(
false
);
if
(
stageMinimal
<
nbStages
()
)
stages
[
stageMinimal
]
->
freezeSlacks
(
true
);
...
...
@@ -597,7 +597,7 @@ namespace soth
{
sotDEBUGPRIOR
(
+
20
);
bool
res
=
true
;
for
(
unsigned
in
t
i
=
0
;
i
<
stages
.
size
();
++
i
)
for
(
stage_sequence_size_
t
i
=
0
;
i
<
stages
.
size
();
++
i
)
{
bool
sres
=
stages
[
i
]
->
testRecomposition
();
if
(
os
&&
(
!
sres
)
)
*
os
<<
"Stage "
<<
i
<<
" is not properly recomposed."
<<
std
::
endl
;
...
...
@@ -611,7 +611,7 @@ namespace soth
{
sotDEBUGPRIOR
(
+
20
);
bool
res
=
true
;
for
(
unsigned
in
t
i
=
0
;
i
<
stages
.
size
();
++
i
)
for
(
stage_sequence_size_
t
i
=
0
;
i
<
stages
.
size
();
++
i
)
{
bool
sres
=
stages
[
i
]
->
testSolution
(
uNext
);
if
(
os
&&
(
!
sres
)
)
*
os
<<
"Stage "
<<
i
<<
" has not been properly inverted."
<<
std
::
endl
;
...
...
@@ -626,20 +626,20 @@ namespace soth
/* Compute sum(i=1:sr) Ji' li, with Jsr = I and lsr = u, and check
* that the result is null. */
bool
HCOD
::
testLagrangeMultipliers
(
int
unsigned
stageRef
,
std
::
ostream
*
os
)
const
testLagrangeMultipliers
(
Index
stageRef
,
std
::
ostream
*
os
)
const
{
assert
(
stageRef
<=
stages
.
size
()
);
VectorXd
verifL
(
sizeProblem
);
/* verifL = Jsr' lsr, with Jsr = I and lsr = u. */
if
(
stageRef
==
stages
.
size
()
)
if
(
stageRef
==
(
Index
)
stages
.
size
()
)
{
verifL
=
solution
;
}
else
verifL
.
setZero
();
/* verif += sum Ji' li. */
const
unsigned
int
nbstage
=
std
::
min
((
unsigned
int
)(
stages
.
size
()
-
1
),
stageRef
);
for
(
unsigned
int
i
=
0
;
i
<=
nbstage
;
++
i
)
const
Index
nbstage
=
std
::
min
((
Index
)(
stages
.
size
()
-
1
),
stageRef
);
for
(
Index
i
=
0
;
i
<=
nbstage
;
++
i
)
{
const
Stage
&
s
=
*
stages
[
i
];
sotDEBUG
(
5
)
<<
"verif = "
<<
(
soth
::
MATLAB
)
verifL
<<
std
::
endl
;
...
...
@@ -651,7 +651,7 @@ namespace soth
sotDEBUG
(
5
)
<<
"verif = "
<<
(
soth
::
MATLAB
)
verifL
<<
std
::
endl
;
// Damping
if
(
nbstage
>
0
&&
stageRef
<
stages
.
size
()
)
if
(
nbstage
>
0
&&
stageRef
<
(
Index
)
stages
.
size
()
)
{
const
Stage
&
s
=
*
stages
[
nbstage
];
if
(
s
.
useDamp
()
)
// && s.sizeN()>0 )
...
...
@@ -679,7 +679,7 @@ namespace soth
show
(
std
::
ostream
&
os
,
bool
check
)
{
sotDEBUGIN
(
15
);
for
(
unsigned
in
t
i
=
0
;
i
<
stages
.
size
();
++
i
)
for
(
stage_sequence_size_
t
i
=
0
;
i
<
stages
.
size
();
++
i
)
{
stages
[
i
]
->
show
(
os
,
i
+
1
,
check
);
}
...
...
@@ -706,7 +706,7 @@ namespace soth
{
sotDEBUGIN
(
15
);
os
<<
"{"
<<
std
::
endl
;
for
(
unsigned
in
t
i
=
0
;
i
<
stages
.
size
();
++
i
)
for
(
stage_sequence_size_
t
i
=
0
;
i
<
stages
.
size
();
++
i
)
{
os
<<
" "
;
stages
[
i
]
->
showActiveSet
(
os
);
os
<<
std
::
endl
;
}
...
...
src/HCOD.hpp
View file @
5adbad1b
...
...
@@ -18,25 +18,26 @@ namespace soth
typedef
stage_sequence_t
::
iterator
stage_iter_t
;
typedef
stage_sequence_t
::
const_iterator
stage_citer_t
;
typedef
stage_sequence_t
::
reverse_iterator
stage_riter_t
;
typedef
stage_sequence_t
::
size_type
stage_sequence_size_t
;
typedef
MatrixXd
::
Index
Index
;
public:
HCOD
(
unsigned
int
sizeProblem
,
unsigned
int
nbStage
=
0
);
HCOD
(
Index
sizeProblem
,
Index
nbStage
=
0
);
void
pushBackStage
(
const
MatrixXd
&
J
,
const
VectorBound
&
bounds
);
void
pushBackStage
(
const
unsigned
int
&
nr
,
const
double
*
Jdata
,
const
Bound
*
bdata
);
void
pushBackStage
(
const
unsigned
int
&
nr
,
const
double
*
Jdata
);
void
pushBackStage
(
const
Index
&
nr
,
const
double
*
Jdata
,
const
Bound
*
bdata
);
void
pushBackStage
(
const
Index
&
nr
,
const
double
*
Jdata
);
void
pushBackStages
(
const
std
::
vector
<
MatrixXd
>
&
J
,
const
std
::
vector
<
VectorBound
>
&
bounds
);
Stage
&
stage
(
unsigned
int
i
);
const
Stage
&
stage
(
unsigned
int
i
)
const
;
inline
Stage
&
operator
[]
(
unsigned
int
i
)
{
return
stage
(
i
);
}
inline
const
Stage
&
operator
[]
(
unsigned
int
i
)
const
{
return
stage
(
i
);
}
Stage
&
stage
(
Index
i
);
const
Stage
&
stage
(
Index
i
)
const
;
inline
Stage
&
operator
[]
(
Index
i
)
{
return
stage
(
i
);
}
inline
const
Stage
&
operator
[]
(
Index
i
)
const
{
return
stage
(
i
);
}
void
setInitialActiveSet
();
void
setInitialActiveSet
(
const
cstref_vector_t
&
Ir0
,
unsigned
int
k
);
cstref_vector_t
getOptimalActiveSet
(
unsigned
int
k
);
void
setInitialActiveSet
(
const
cstref_vector_t
&
Ir0
,
Index
k
);
cstref_vector_t
getOptimalActiveSet
(
Index
k
);
std
::
vector
<
cstref_vector_t
>
getOptimalActiveSet
();
void
setInitialActiveSet
(
const
std
::
vector
<
cstref_vector_t
>
&
Ir
);
...
...
@@ -48,16 +49,16 @@ namespace soth
//sizes
Index
sizeA
()
const
;
int
rank
()
const
;
unsigned
int
nbStages
()
const
{
return
(
unsigned
int
)
stages
.
size
();
}
Index
nbStages
()
const
{
return
(
Index
)
stages
.
size
();
}
/* --- Decomposition --- */
public:
void
reset
(
void
);
void
initialize
(
void
);
void
update
(
const
unsigned
int
&
stageUp
,
const
ConstraintRef
&
cst
);
void
update
(
const
Index
&
stageUp
,
const
ConstraintRef
&
cst
);
void
update
(
stage_iter_t
stageIter
,
const
ConstraintRef
&
cst
);
void
downdate
(
const
unsigned
int
&
stageDown
,
const
unsigned
int
&
row
);
void
downdate
(
stage_iter_t
stageIter
,
const
unsigned
int
&
row
);
void
downdate
(
const
Index
&
stageDown
,
const
Index
&
row
);