Handle(Geom_BSplineCurve) CFunctionToBspline::CFunctionToBsplineImpl::concatC1(const std::vector<Handle(Geom_BSplineCurve)>& curves)
{
if (curves.size() == 0) {
return NULL;
}
else if (curves.size() == 1) {
return curves[0];
}
#ifdef DEBUG
// check range connectivities
for (size_t i = 1; i < curves.size(); ++i) {
Handle(Geom_BSplineCurve) lastCurve = curves[i-1];
Handle(Geom_BSplineCurve) thisCurve = curves[i];
assert(lastCurve->LastParameter() == thisCurve->FirstParameter());
}
#endif
// count control points
int ncp = 2;
int nkn = 1;
std::vector<Handle(Geom_BSplineCurve)>::const_iterator curveIt;
for (curveIt = curves.begin(); curveIt != curves.end(); ++curveIt) {
Handle(Geom_BSplineCurve) curve = *curveIt;
ncp += curve->NbPoles() - 2;
nkn += curve->NbKnots() - 1;
}
// allocate arrays
TColgp_Array1OfPnt cpoints(1, ncp);
TColStd_Array1OfReal knots(1, nkn);
TColStd_Array1OfInteger mults(1, nkn);
int iknotT = 1, imultT = 1, icpT = 1;
int icurve = 0;
for (curveIt = curves.begin(); curveIt != curves.end(); ++curveIt, ++icurve) {
Handle(Geom_BSplineCurve) curve = *curveIt;
// special handling of the first knot, control point
knots.SetValue(iknotT++, curve->Knot(1));
if (icurve == 0) {
// we just copy the data of the very first point/knot
mults.SetValue(imultT++, curve->Multiplicity(1));
cpoints.SetValue(icpT++, curve->Pole(1));
}
else {
// set multiplicity to maxDegree to allow c0 concatenation
mults.SetValue(imultT++, _degree-1);
// omit the first control points of the current curve
}
// just copy control points, weights, knots and multiplicites
for (int iknot = 2; iknot < curve->NbKnots(); ++iknot) {
knots.SetValue(iknotT++, curve->Knot(iknot));
mults.SetValue(imultT++, curve->Multiplicity(iknot));
}
for (int icp = 2; icp < curve->NbPoles(); ++icp) {
cpoints.SetValue(icpT++, curve->Pole(icp));
}
}
// special handling of the last point and knot
Handle(Geom_BSplineCurve) lastCurve = curves[curves.size()-1];
knots.SetValue(iknotT, lastCurve->Knot(lastCurve->NbKnots()));
mults.SetValue(imultT, lastCurve->Multiplicity(lastCurve->NbKnots()));
cpoints.SetValue(icpT, lastCurve->Pole(lastCurve->NbPoles()));
Handle(Geom_BSplineCurve) result = new Geom_BSplineCurve(cpoints, knots, mults, _degree);
return result;
}
int main()
{
int j,k;
big a,b,x,y,p,A2;
time_t seed;
epoint *g;
double tr1,tr2,ts,tv1,tv2,tp,td;
#ifndef MR_NOFULLWIDTH
miracl *mip=mirsys(300,0);
#else
miracl *mip=mirsys(300,MAXBASE);
#endif
p=mirvar(0);
a=mirvar(-3);
b=mirvar(0);
x=mirvar(1);
y=mirvar(0);
A2=mirvar(0);
mip->IOBASE=60;
time(&seed);
irand((long)seed);
printf("MIRACL - %d bit version\n",MIRACL);
#ifdef MR_LITTLE_ENDIAN
printf("Little Endian processor\n");
#endif
#ifdef MR_BIG_ENDIAN
printf("Big Endian processor\n");
#endif
#ifdef MR_NOASM
printf("C-Only Version of MIRACL\n");
#else
printf("Using some assembly language\n");
#endif
#ifdef MR_STRIPPED_DOWN
printf("Stripped down version of MIRACL - no error messages\n");
#endif
#ifdef MR_KCM
k=MR_KCM*MIRACL;
printf("Using KCM method \n");
printf("Optimized for %d, %d, %d, %d...etc. bit moduli\n",k,k*2,k*4,k*8);
#endif
#ifdef MR_COMBA
k=MR_COMBA*MIRACL;
printf("Using COMBA method \n");
printf("Optimized for %d bit moduli\n",k);
#endif
#ifdef MR_PENTIUM
printf("Floating-point co-processor arithmetic used for Pentium\n");
#endif
#ifndef MR_KCM
#ifndef MR_COMBA
#ifndef MR_PENTIUM
printf("No special optimizations\n");
#endif
#endif
#endif
printf("Precomputation uses fixed Window size = %d\n",WINDOW);
printf("So %d values are precomputed and stored\n",(1<<WINDOW));
#ifdef MR_NOFULLWIDTH
printf("No Fullwidth base possible\n");
#else
printf("NOTE: No optimizations/assembly language apply to GF(2^m) Elliptic Curves\n");
#endif
printf("NOTE: times are elapsed real-times - so make sure nothing else is running!\n\n");
printf("Modular exponentiation benchmarks - calculating g^e mod p\n");
printf("From these figures it should be possible to roughly estimate the time\n");
printf("required for your favourite PK algorithm, RSA, DSA, DH, etc.\n");
printf("Key R - random base bits/random exponent bits \n");
printf(" V - random base bits/(small exponent e) \n");
printf(" S - (small base g) /random exponent bits \n");
printf(" P - exponentiation with precomputation (fixed base g)\n");
printf(" D - double exponentiation g^e.a^b mod p\n");
printf("F3 = 257, F4 = 65537\n");
printf("RSA - Rivest-Shamir-Adleman\n");
printf("DH - Diffie Hellman Key exchange\n");
printf("DSA - Digital Signature Algorithm\n");
printf("\n512 bit prime....\n");
cinstr(p,p512);
k=512;
j=160;
tr1=powers(k,j,p);
td=powers_double(k,j,p);
tr2=powers(k,k,p);
ts=powers_small_base(3,j,p);
tp=powers_precomp(k,j,p);
printf("\n");
printf("%4d bit RSA decryption %8.2lf ms \n",2*k,2*tr2);
printf("%4d bit DH %d bit exponent:-\n",k,j);
printf(" offline, no precomputation %8.2lf ms \n",tr1);
printf(" offline, small base %8.2lf ms \n",ts);
printf(" offline, w. precomputation %8.2lf ms \n",tp);
printf(" online %8.2lf ms \n",tr1);
printf("%4d bit DSA %d bit exponent:-\n",k,j);
printf(" signature no precomputation %8.2lf ms \n",tr1);
printf(" signature w. precomputation %8.2lf ms \n",tp);
printf(" verification %8.2lf ms \n",td);
printf("\n1024 bit prime....\n");
cinstr(p,p1024);
k=1024; j=160;
tr1=powers(k,j,p);
td=powers_double(k,j,p);
tr2=powers(k,k,p);
tv1=powers_small_exp(k,3,p);
tv2=powers_small_exp(k,65537L,p);
ts=powers_small_base(3,j,p);
tp=powers_precomp(k,j,p);
printf("\n");
printf("%4d bit RSA decryption %8.2lf ms \n",2*k,2*tr2);
printf("%4d bit RSA encryption e=3 %8.2lf ms \n",k,tv1);
printf("%4d bit RSA encryption e=65537 %8.2lf ms \n",k,tv2);
printf("%4d bit DH %d bit exponent:-\n",k,j);
printf(" offline, no precomputation %8.2lf ms \n",tr1);
printf(" offline, small base %8.2lf ms \n",ts);
printf(" offline, w. precomputation %8.2lf ms \n",tp);
printf(" online %8.2lf ms \n",tr1);
printf("%4d bit DSA %d bit exponent:-\n",k,j);
printf(" signature no precomputation %8.2lf ms \n",tr1);
printf(" signature w. precomputation %8.2lf ms \n",tp);
printf(" verification %8.2lf ms \n",td);
printf("\n2048 bit prime....\n");
cinstr(p,p2048);
k=2048; j=256;
tr1=powers(k,j,p);
td=powers_double(k,j,p);
powers(k,k,p);
tv1=powers_small_exp(k,3,p);
tv2=powers_small_exp(k,65537L,p);
ts=powers_small_base(3,j,p);
tp=powers_precomp(k,j,p);
printf("\n");
printf("%4d bit RSA encryption e=3 %8.2lf ms \n",k,tv1);
printf("%4d bit RSA encryption e=65537 %8.2lf ms \n",k,tv2);
printf("%4d bit DH %d bit exponent:-\n",k,j);
printf(" offline, no precomputation %8.2lf ms \n",tr1);
printf(" offline, small base %8.2lf ms \n",ts);
printf(" offline, w. precomputation %8.2lf ms \n",tp);
printf(" online %8.2lf ms \n",tr1);
printf("%4d bit DSA %d bit exponent:-\n",k,j);
printf(" signature no precomputation %8.2lf ms \n",tr1);
printf(" signature w. precomputation %8.2lf ms \n",tp);
printf(" verification %8.2lf ms \n",td);
printf("\n");
printf("Elliptic Curve point multiplication benchmarks - calculating r.P\n");
printf("From these figures it should be possible to roughly estimate the time\n");
printf("required for your favourite EC PK algorithm, ECDSA, ECDH, etc.\n");
printf("Key - ER - Elliptic Curve point multiplication r.P\n");
printf(" ED - Elliptic Curve double multiplication r.P + s.Q\n");
printf(" EP - Elliptic Curve multiplication with precomputation\n");
printf("EC - Elliptic curve GF(p) - p of no special form \n");
printf("ECDH - Diffie Hellman Key exchange\n");
printf("ECDSA - Digital Signature Algorithm\n");
mip->IOBASE=10;
printf("\n160 bit GF(p) Elliptic Curve....\n");
k=160;
cinstr(p,p160);
cinstr(b,b160);
cinstr(y,y160);
ecurve_init(a,b,p,MR_PROJECTIVE);
g=epoint_init();
if (!epoint_set(x,y,0,g))
{
printf("This is not a point on the curve!\n");
exit(0);
}
tr1=mults(k,g);
td=mult_double(k,g);
tp=mult_precomp(k,x,y,a,b,p);
printf("\n");
printf("%4d bit ECDH :-\n",k);
printf(" offline, no precomputation %8.2lf ms \n",tr1);
printf(" offline, w. precomputation %8.2lf ms \n",tp);
printf(" online %8.2lf ms \n",tr1);
printf("%4d bit ECDSA :-\n",k);
printf(" signature no precomputation %8.2lf ms \n",tr1);
printf(" signature w. precomputation %8.2lf ms \n",tp);
printf(" verification %8.2lf ms \n",td);
printf("\n192 bit GF(p) Elliptic Curve....\n");
k=192;
cinstr(p,p192);
cinstr(b,b192);
cinstr(y,y192);
ecurve_init(a,b,p,MR_PROJECTIVE);
g=epoint_init();
if (!epoint_set(x,y,0,g))
{
printf("This is not a point on the curve!\n");
exit(0);
}
tr1=mults(k,g);
td=mult_double(k,g);
tp=mult_precomp(k,x,y,a,b,p);
printf("\n");
printf("%4d bit ECDH :-\n",k);
printf(" offline, no precomputation %8.2lf ms \n",tr1);
printf(" offline, w. precomputation %8.2lf ms \n",tp);
printf(" online %8.2lf ms \n",tr1);
printf("%4d bit ECDSA :-\n",k);
printf(" signature no precomputation %8.2lf ms \n",tr1);
printf(" signature w. precomputation %8.2lf ms \n",tp);
printf(" verification %8.2lf ms \n",td);
printf("\n224 bit GF(p) Elliptic Curve....\n");
k=224;
cinstr(p,p224);
cinstr(b,b224);
cinstr(y,y224);
ecurve_init(a,b,p,MR_PROJECTIVE);
g=epoint_init();
if (!epoint_set(x,y,0,g))
{
printf("This is not a point on the curve!\n");
exit(0);
}
tr1=mults(k,g);
td=mult_double(k,g);
tp=mult_precomp(k,x,y,a,b,p);
printf("\n");
printf("%4d bit ECDH :-\n",k);
printf(" offline, no precomputation %8.2lf ms \n",tr1);
printf(" offline, w. precomputation %8.2lf ms \n",tp);
printf(" online %8.2lf ms \n",tr1);
printf("%4d bit ECDSA :-\n",k);
printf(" signature no precomputation %8.2lf ms \n",tr1);
printf(" signature w. precomputation %8.2lf ms \n",tp);
printf(" verification %8.2lf ms \n",td);
printf("\n256 bit GF(p) Elliptic Curve....\n");
k=256;
cinstr(p,p256);
cinstr(b,b256);
cinstr(y,y256);
ecurve_init(a,b,p,MR_PROJECTIVE);
g=epoint_init();
if (!epoint_set(x,y,0,g))
{
printf("This is not a point on the curve!\n");
exit(0);
}
tr1=mults(k,g);
td=mult_double(k,g);
tp=mult_precomp(k,x,y,a,b,p);
printf("\n");
printf("%4d bit ECDH :-\n",k);
printf(" offline, no precomputation %8.2lf ms \n",tr1);
printf(" offline, w. precomputation %8.2lf ms \n",tp);
printf(" online %8.2lf ms \n",tr1);
printf("%4d bit ECDSA :-\n",k);
printf(" signature no precomputation %8.2lf ms \n",tr1);
printf(" signature w. precomputation %8.2lf ms \n",tp);
printf(" verification %8.2lf ms \n",td);
#ifndef MR_FP
printf("\n163 bit GF(2^m) Elliptic Curve....\n");
k=163;
mip->IOBASE=16;
cinstr(b,B163);
cinstr(x,x163);
cinstr(y,y163);
mip->IOBASE=10;
convert(A163,A2);
ecurve2_init(m163,a163,b163,c163,A2,b,FALSE,MR_PROJECTIVE);
g=epoint_init();
if (!epoint2_set(x,y,0,g))
{
printf("This is not a point on the curve!\n");
exit(0);
}
tr1=mults2(k,g);
td=mult2_double(k,g);
tp=mult2_precomp(k,x,y,A2,b,m163,a163,b163,c163);
printf("\n");
printf("%4d bit ECDH :-\n",k);
printf(" offline, no precomputation %8.2lf ms \n",tr1);
printf(" offline, w. precomputation %8.2lf ms \n",tp);
printf(" online %8.2lf ms \n",tr1);
printf("%4d bit ECDSA :-\n",k);
printf(" signature no precomputation %8.2lf ms \n",tr1);
printf(" signature w. precomputation %8.2lf ms \n",tp);
printf(" verification %8.2lf ms \n",td);
printf("\n163 bit GF(2^m) Koblitz Elliptic Curve....\n");
k=163;
mip->IOBASE=16;
cinstr(b,KB163);
cinstr(x,Kx163);
cinstr(y,Ky163);
mip->IOBASE=10;
convert(KA163,A2);
ecurve2_init(m163,a163,b163,c163,A2,b,FALSE,MR_PROJECTIVE);
g=epoint_init();
if (!epoint2_set(x,y,0,g))
{
printf("This is not a point on the curve!\n");
exit(0);
}
tr1=mults2(k,g);
td=mult2_double(k,g);
tp=mult2_precomp(k,x,y,A2,b,m163,a163,b163,c163);
printf("\n");
printf("%4d bit ECDH :-\n",k);
printf(" offline, no precomputation %8.2lf ms \n",tr1);
printf(" offline, w. precomputation %8.2lf ms \n",tp);
printf(" online %8.2lf ms \n",tr1);
printf("%4d bit ECDSA :-\n",k);
printf(" signature no precomputation %8.2lf ms \n",tr1);
printf(" signature w. precomputation %8.2lf ms \n",tp);
printf(" verification %8.2lf ms \n",td);
printf("\n233 bit GF(2^m) Elliptic Curve....\n");
k=233;
mip->IOBASE=16;
cinstr(b,B233);
cinstr(x,x233);
cinstr(y,y233);
mip->IOBASE=10;
convert(A233,A2);
ecurve2_init(m233,a233,b233,c233,A2,b,FALSE,MR_PROJECTIVE);
g=epoint_init();
if (!epoint2_set(x,y,0,g))
{
printf("This is not a point on the curve!\n");
exit(0);
}
tr1=mults2(k,g);
td=mult2_double(k,g);
tp=mult2_precomp(k,x,y,A2,b,m233,a233,b233,c233);
printf("\n");
printf("%4d bit ECDH :-\n",k);
printf(" offline, no precomputation %8.2lf ms \n",tr1);
printf(" offline, w. precomputation %8.2lf ms \n",tp);
printf(" online %8.2lf ms \n",tr1);
printf("%4d bit ECDSA :-\n",k);
printf(" signature no precomputation %8.2lf ms \n",tr1);
printf(" signature w. precomputation %8.2lf ms \n",tp);
printf(" verification %8.2lf ms \n",td);
printf("\n233 bit GF(2^m) Koblitz Elliptic Curve....\n");
k=233;
mip->IOBASE=16;
cinstr(b,KB233);
cinstr(x,Kx233);
cinstr(y,Ky233);
mip->IOBASE=10;
convert(KA233,A2);
ecurve2_init(m233,a233,b233,c233,A2,b,FALSE,MR_PROJECTIVE);
g=epoint_init();
if (!epoint2_set(x,y,0,g))
{
printf("This is not a point on the curve!\n");
exit(0);
}
tr1=mults2(k,g);
td=mult2_double(k,g);
tp=mult2_precomp(k,x,y,A2,b,m233,a233,b233,c233);
printf("\n");
printf("%4d bit ECDH :-\n",k);
printf(" offline, no precomputation %8.2lf ms \n",tr1);
printf(" offline, w. precomputation %8.2lf ms \n",tp);
printf(" online %8.2lf ms \n",tr1);
printf("%4d bit ECDSA :-\n",k);
printf(" signature no precomputation %8.2lf ms \n",tr1);
printf(" signature w. precomputation %8.2lf ms \n",tp);
printf(" verification %8.2lf ms \n",td);
printf("\n283 bit GF(2^m) Elliptic Curve....\n");
k=283;
mip->IOBASE=16;
cinstr(b,B283);
cinstr(x,x283);
cinstr(y,y283);
mip->IOBASE=10;
convert(A283,A2);
ecurve2_init(m283,a283,b283,c283,A2,b,FALSE,MR_PROJECTIVE);
g=epoint_init();
if (!epoint2_set(x,y,0,g))
{
printf("This is not a point on the curve!\n");
exit(0);
}
tr1=mults2(k,g);
td=mult2_double(k,g);
tp=mult2_precomp(k,x,y,A2,b,m283,a283,b283,c283);
printf("\n");
printf("%4d bit ECDH :-\n",k);
printf(" offline, no precomputation %8.2lf ms \n",tr1);
printf(" offline, w. precomputation %8.2lf ms \n",tp);
printf(" online %8.2lf ms \n",tr1);
printf("%4d bit ECDSA :-\n",k);
printf(" signature no precomputation %8.2lf ms \n",tr1);
printf(" signature w. precomputation %8.2lf ms \n",tp);
printf(" verification %8.2lf ms \n",td);
printf("\n283 bit GF(2^m) Koblitz Elliptic Curve....\n");
k=283;
mip->IOBASE=16;
cinstr(b,KB283);
cinstr(x,Kx283);
cinstr(y,Ky283);
mip->IOBASE=10;
convert(KA283,A2);
ecurve2_init(m283,a283,b283,c283,A2,b,FALSE,MR_PROJECTIVE);
g=epoint_init();
if (!epoint2_set(x,y,0,g))
{
printf("This is not a point on the curve!\n");
exit(0);
}
tr1=mults2(k,g);
td=mult2_double(k,g);
tp=mult2_precomp(k,x,y,A2,b,m283,a283,b283,c283);
printf("\n");
printf("%4d bit ECDH :-\n",k);
printf(" offline, no precomputation %8.2lf ms \n",tr1);
printf(" offline, w. precomputation %8.2lf ms \n",tp);
printf(" online %8.2lf ms \n",tr1);
printf("%4d bit ECDSA :-\n",k);
printf(" signature no precomputation %8.2lf ms \n",tr1);
printf(" signature w. precomputation %8.2lf ms \n",tp);
printf(" verification %8.2lf ms \n",td);
printf("\n571 bit GF(2^m) Elliptic Curve....\n");
k=571;
mip->IOBASE=16;
cinstr(b,B571);
cinstr(x,x571);
cinstr(y,y571);
mip->IOBASE=10;
convert(A571,A2);
ecurve2_init(m571,a571,b571,c571,A2,b,FALSE,MR_PROJECTIVE);
g=epoint_init();
if (!epoint2_set(x,y,0,g))
{
printf("This is not a point on the curve!\n");
exit(0);
}
tr1=mults2(k,g);
td=mult2_double(k,g);
tp=mult2_precomp(k,x,y,A2,b,m571,a571,b571,c571);
printf("\n");
printf("%4d bit ECDH :-\n",k);
printf(" offline, no precomputation %8.2lf ms \n",tr1);
printf(" offline, w. precomputation %8.2lf ms \n",tp);
printf(" online %8.2lf ms \n",tr1);
printf("%4d bit ECDSA :-\n",k);
printf(" signature no precomputation %8.2lf ms \n",tr1);
printf(" signature w. precomputation %8.2lf ms \n",tp);
printf(" verification %8.2lf ms \n",td);
printf("\n571 bit GF(2^m) Koblitz Elliptic Curve....\n");
k=571;
mip->IOBASE=16;
cinstr(b,KB571);
cinstr(x,Kx571);
cinstr(y,Ky571);
mip->IOBASE=10;
convert(KA571,A2);
ecurve2_init(m571,a571,b571,c571,A2,b,FALSE,MR_PROJECTIVE);
g=epoint_init();
if (!epoint2_set(x,y,0,g))
{
printf("This is not a point on the curve!\n");
exit(0);
}
tr1=mults2(k,g);
td=mult2_double(k,g);
tp=mult2_precomp(k,x,y,A2,b,m571,a571,b571,c571);
printf("\n");
printf("%4d bit ECDH :-\n",k);
printf(" offline, no precomputation %8.2lf ms \n",tr1);
printf(" offline, w. precomputation %8.2lf ms \n",tp);
printf(" online %8.2lf ms \n",tr1);
printf("%4d bit ECDSA :-\n",k);
printf(" signature no precomputation %8.2lf ms \n",tr1);
printf(" signature w. precomputation %8.2lf ms \n",tp);
printf(" verification %8.2lf ms \n",td);
#endif
return 0;
}
Handle(Geom_BSplineCurve) CFunctionToBspline::CFunctionToBsplineImpl::Curve()
{
bool interpolate = true;
std::vector<ChebSegment> segments = approxSegment(_umin, _umax, 1);
int N = _degree + 1;
math_Matrix Mt = monimial_to_bezier(N)*cheb_to_monomial(N);
// get estimated error and create bspline segments
std::vector<Handle(Geom_BSplineCurve)> curves;
double errTotal = 0.;
std::vector<ChebSegment>::iterator it = segments.begin();
for (; it != segments.end(); ++it) {
// get control points
ChebSegment& seg = *it;
math_Vector cpx = Mt*seg.cx;
math_Vector cpy = Mt*seg.cy;
math_Vector cpz = Mt*seg.cz;
TColgp_Array1OfPnt cp(1,cpx.Length());
for (int i = 1; i <= cpx.Length(); ++i) {
gp_Pnt p(cpx(i-1), cpy(i-1), cpz(i-1));
cp.SetValue(i, p);
}
if (interpolate) {
gp_Pnt pstart(_xfunc.value(seg.umin), _yfunc.value(seg.umin), _zfunc.value(seg.umin));
gp_Pnt pstop (_xfunc.value(seg.umax), _yfunc.value(seg.umax), _zfunc.value(seg.umax));
cp.SetValue(1, pstart);
cp.SetValue(cpx.Length(), pstop);
}
// create knots and multiplicity vector
TColStd_Array1OfReal knots(1,2);
knots.SetValue(1, seg.umin);
knots.SetValue(2, seg.umax);
TColStd_Array1OfInteger mults(1,2);
mults.SetValue(1, _degree+1);
mults.SetValue(2, _degree+1);
Handle(Geom_BSplineCurve) curve = new Geom_BSplineCurve(cp, knots, mults, _degree);
curves.push_back(curve);
if (seg.error > errTotal) {
errTotal = seg.error;
}
}
_err = errTotal;
// concatenate c1 the bspline curves
Handle(Geom_BSplineCurve) result = concatC1(curves);
#ifdef DEBUG
LOG(INFO) << "Result of BSpline approximation of function:";
LOG(INFO) << " approximation error = " << errTotal;
LOG(INFO) << " number of control points = " << result->NbPoles();
LOG(INFO) << " number of segments = " << curves.size();
#endif
return result;
}
//------------------------------------------------------------------------------
// SOLVE BILATERAL
//------------------------------------------------------------------------------
bool PGSImpulseSolver::
solveBilateral
(const Array_<MultiplierIndex>& participating, // p<=m of these
const Matrix& A, // m X m, symmetric
const Vector& D, // m, diag>=0 added to A
const Vector& rhs, // m, RHS
Vector& pi // m, unknown result
) const
{
SimTK_DEBUG("--------------------------------\n");
SimTK_DEBUG( "PGS BILATERAL SOLVER:\n");
++m_nBilateralSolves;
const int m=A.nrow();
const int p = (int)participating.size();
assert(A.ncol()==m);
assert(D.size()==0 || D.size()==m);
assert(rhs.size()==m);
assert(p<=m);
pi.resize(m);
pi.setToZero(); // That takes care of all non-participators.
if (p == 0) {
SimTK_DEBUG(" no bilateral participators. Nothing to do.\n");
SimTK_DEBUG("--------------------------------\n");
return true;
}
// Track total error for all included equations, and the error for just
// those equations that are being enforced.
bool converged = false;
Real normRMSenf = Infinity, sor = m_SOR;
Real prevNormRMSenf = NaN;
int its = 1;
Array_<Real> rowSums; // handy temp
for (; its <= m_maxIters; ++its) {
++m_nBilateralIters;
Real sum2enf = 0; // track solution errors
prevNormRMSenf = normRMSenf;
// All the participating constraints are unconditionally active.
Array_<MultiplierIndex> mults(1);
for (int k=0; k < p; ++k) {
mults[0] = participating[k];
doRowSums(participating,mults,A,D,pi,rowSums);
const Real localEr2=doUpdates(mults,A,D,rhs,sor,rowSums,pi);
sum2enf += localEr2;
}
normRMSenf = std::sqrt(sum2enf/p);
const Real rate = normRMSenf/prevNormRMSenf;
if (rate > 1) {
SimTK_DEBUG3("GOT WORSE@%d: sor=%g rate=%g\n", its, sor, rate);
if (sor > .1)
sor = std::max(.8*sor, .1);
}
#ifndef NDEBUG
printf("iter %d: EST rmsEnf=%g rate=%g\n", its,
normRMSenf, normRMSenf/prevNormRMSenf);
#endif
if (normRMSenf < m_convergenceTol) //TODO: add failure-to-improve check
{
SimTK_DEBUG2("BILATERAL PGS converged to %g in %d iters\n",
normRMSenf, its);
converged = true;
break;
}
#ifndef NDEBUG
cout << "pi=" << pi << " err=" << normRMSenf << " rate=" << rate << endl;
#endif
}
if (!converged) {
SimTK_DEBUG2("BILATERAL PGS CONVERGENCE FAILURE: %d iters -> norm=%g\n",
its, normRMSenf);
++m_nBilateralFail;
}
#ifndef NDEBUG
cout << "A=" << A;
cout << "D=" << D << endl;
cout << "rhs=" << rhs << endl;
cout << "active=" << participating << endl;
cout << "-> pi=" << pi << endl;
if (D.size()) cout << "resid=" << A*pi+D.elementwiseMultiply(pi)-rhs << endl;
else cout << "resid=" << A*pi-rhs << endl;
#endif
SimTK_DEBUG("--------------------------------\n");
return converged;
}
